﻿using HalconDotNet;
using Prism.Ioc;
using System.Collections.ObjectModel;
using System;
using ViewROI;
using DualPlaneFR4Paster.Common.Services;
using Prism.Commands;
using System.Diagnostics;
using System.Collections.Concurrent;
using MvCamCtrl.NET.CameraParams;
using MvCamCtrl.NET;
using DualPlaneFR4Paster.Models;
using System.Linq;
using DualPlaneFR4Paster.Extensions;
using System.Threading.Tasks;
using System.Threading;
using System.Collections.Generic;
using Newtonsoft.Json;
using Prism.Services.Dialogs;
using System.Reflection.Metadata;
using DualPlaneFR4Paster.Common.Events;
using System.Windows.Forms;
using System.IO;

namespace DualPlaneFR4Paster.ViewModels.Home
{
    public class BottomFloorViewModel : NavigationViewModel
    {
        #region 变量
        private IParamService paramService;
        private IGTSCardService Top_A4;
        private IGTSCardService Bottom_A8;
        private ICameraService camDown, camUp;
        private IServoModbusService servoModbus;
        private IPLCModbusService plcModbus;
        private CancellationTokenSource source;
        private CancellationTokenSource source1;
        double z1SafePos = 0, z2SafePos = 0, z3SafePos = 0, z4SafePos = 0;
        double xRunSpeed = 0, yRunSpeed = 0, zRunSpeed = 0, rRunSpeed = 0;
        double xYGapLimit = 0, rGapLimit = 0;
        float upCameraExposureTime = 0, downCameraExposureTime = 0;
        double filmSuckTime = 0, filmReleaseTime = 0, zSlowDistSuck = 0, zSlowDistPaste = 0, zSlowSpeedSuck = 0, zSlowSpeedPaste = 0;
        XYZRPoint initPoint, suckPicker1, suckPicker2, suckPicker3, suckPicker4, flyStartPicker, flyEndPicker, buttomCameraGrab_Sucker1,
            centerSuck2, centerSuck3, centerSuck4, calibpoint, bottomCameraFail_Sucker1, topCameraMark1, topCameraMark2;
        SuckerWork[] suckerWorkArr;
        FeederWork[] feederWorkArr;
        bool XHomed = false, YHomed = false;
        bool PauseFlag = false;
        bool RailReadyFlag = false, AssistFinished = false, TopWordFinished = false;
        bool IsSimulate = false;
        double downCameraGrabPreTime = 0, upCameraGrabPreTime = 0;
        bool startFlag = false;
        HObject Mark1Region, Mark2Region;
        HTuple UpCameraCalibHomMat2D, Mark1Row, Mark1Column, Mark2Row, Mark2Column, MarkMidX0, MarkMidY0, MarkMidR0;
        double Mark1ThresholdMaxGray = 0, Mark1MinArea = 0, Mark1MaxArea = 0, Mark2ThresholdMaxGray = 0, Mark2MinArea = 0, Mark2MaxArea = 0;
        HObject[] recognizeRegions, ModelContourss; HTuple[] ModelIDs, ShapeModelRows, ShapeModelColumns, ShapeModelAngles, StandardRows, StandardColumns, StandardAngles;
        ROILine[] roiLine1s, roiLine2s;
        HTuple CalibHomMat2D;
        #endregion
        #region 属性
        #region halcon
        #region 1#
        private HImage cameraIamge1;
        public HImage CameraIamge1
        {
            get { return cameraIamge1; }
            set { SetProperty(ref cameraIamge1, value); }
        }
        private bool cameraRepaint1;
        public bool CameraRepaint1
        {
            get { return cameraRepaint1; }
            set { SetProperty(ref cameraRepaint1, value); }
        }
        private ObservableCollection<ROI> cameraROIList1 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList1
        {
            get { return cameraROIList1; }
            set { SetProperty(ref cameraROIList1, value); }
        }
        private HObject cameraAppendHObject1;
        public HObject CameraAppendHObject1
        {
            get { return cameraAppendHObject1; }
            set { SetProperty(ref cameraAppendHObject1, value); }
        }
        private HMsgEntry cameraAppendHMessage1;
        public HMsgEntry CameraAppendHMessage1
        {
            get { return cameraAppendHMessage1; }
            set { SetProperty(ref cameraAppendHMessage1, value); }
        }
        private Tuple<string, object> cameraGCStyle1;
        public Tuple<string, object> CameraGCStyle1
        {
            get { return cameraGCStyle1; }
            set { SetProperty(ref cameraGCStyle1, value); }
        }
        #endregion
        #region 2#
        private HImage cameraIamge2;
        public HImage CameraIamge2
        {
            get { return cameraIamge2; }
            set { SetProperty(ref cameraIamge2, value); }
        }
        private bool cameraRepaint2;
        public bool CameraRepaint2
        {
            get { return cameraRepaint2; }
            set { SetProperty(ref cameraRepaint2, value); }
        }
        private ObservableCollection<ROI> cameraROIList2 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList2
        {
            get { return cameraROIList2; }
            set { SetProperty(ref cameraROIList2, value); }
        }
        private HObject cameraAppendHObject2;
        public HObject CameraAppendHObject2
        {
            get { return cameraAppendHObject2; }
            set { SetProperty(ref cameraAppendHObject2, value); }
        }
        private HMsgEntry cameraAppendHMessage2;
        public HMsgEntry CameraAppendHMessage2
        {
            get { return cameraAppendHMessage2; }
            set { SetProperty(ref cameraAppendHMessage2, value); }
        }
        private Tuple<string, object> cameraGCStyle2;
        public Tuple<string, object> CameraGCStyle2
        {
            get { return cameraGCStyle2; }
            set { SetProperty(ref cameraGCStyle2, value); }
        }
        #endregion
        #region 3#
        private HImage cameraIamge3;
        public HImage CameraIamge3
        {
            get { return cameraIamge3; }
            set { SetProperty(ref cameraIamge3, value); }
        }
        private bool cameraRepaint3;
        public bool CameraRepaint3
        {
            get { return cameraRepaint3; }
            set { SetProperty(ref cameraRepaint3, value); }
        }
        private ObservableCollection<ROI> cameraROIList3 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList3
        {
            get { return cameraROIList3; }
            set { SetProperty(ref cameraROIList3, value); }
        }
        private HObject cameraAppendHObject3;
        public HObject CameraAppendHObject3
        {
            get { return cameraAppendHObject3; }
            set { SetProperty(ref cameraAppendHObject3, value); }
        }
        private HMsgEntry cameraAppendHMessage3;
        public HMsgEntry CameraAppendHMessage3
        {
            get { return cameraAppendHMessage3; }
            set { SetProperty(ref cameraAppendHMessage3, value); }
        }
        private Tuple<string, object> cameraGCStyle3;
        public Tuple<string, object> CameraGCStyle3
        {
            get { return cameraGCStyle3; }
            set { SetProperty(ref cameraGCStyle3, value); }
        }
        #endregion
        #region 4#
        private HImage cameraIamge4;
        public HImage CameraIamge4
        {
            get { return cameraIamge4; }
            set { SetProperty(ref cameraIamge4, value); }
        }
        private bool cameraRepaint4;
        public bool CameraRepaint4
        {
            get { return cameraRepaint4; }
            set { SetProperty(ref cameraRepaint4, value); }
        }
        private ObservableCollection<ROI> cameraROIList4 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList4
        {
            get { return cameraROIList4; }
            set { SetProperty(ref cameraROIList4, value); }
        }
        private HObject cameraAppendHObject4;
        public HObject CameraAppendHObject4
        {
            get { return cameraAppendHObject4; }
            set { SetProperty(ref cameraAppendHObject4, value); }
        }
        private HMsgEntry cameraAppendHMessage4;
        public HMsgEntry CameraAppendHMessage4
        {
            get { return cameraAppendHMessage4; }
            set { SetProperty(ref cameraAppendHMessage4, value); }
        }
        private Tuple<string, object> cameraGCStyle4;
        public Tuple<string, object> CameraGCStyle4
        {
            get { return cameraGCStyle4; }
            set { SetProperty(ref cameraGCStyle4, value); }
        }
        #endregion
        #region 5#
        private HImage cameraIamge5;
        public HImage CameraIamge5
        {
            get { return cameraIamge5; }
            set { SetProperty(ref cameraIamge5, value); }
        }
        private bool cameraRepaint5;
        public bool CameraRepaint5
        {
            get { return cameraRepaint5; }
            set { SetProperty(ref cameraRepaint5, value); }
        }
        private ObservableCollection<ROI> cameraROIList5 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList5
        {
            get { return cameraROIList5; }
            set { SetProperty(ref cameraROIList5, value); }
        }
        private HObject cameraAppendHObject5;
        public HObject CameraAppendHObject5
        {
            get { return cameraAppendHObject5; }
            set { SetProperty(ref cameraAppendHObject5, value); }
        }
        private HMsgEntry cameraAppendHMessage5;
        public HMsgEntry CameraAppendHMessage5
        {
            get { return cameraAppendHMessage5; }
            set { SetProperty(ref cameraAppendHMessage5, value); }
        }
        private Tuple<string, object> cameraGCStyle5;
        public Tuple<string, object> CameraGCStyle5
        {
            get { return cameraGCStyle5; }
            set { SetProperty(ref cameraGCStyle5, value); }
        }
        #endregion
        #region 6#
        private HImage cameraIamge6;
        public HImage CameraIamge6
        {
            get { return cameraIamge6; }
            set { SetProperty(ref cameraIamge6, value); }
        }
        private bool cameraRepaint6;
        public bool CameraRepaint6
        {
            get { return cameraRepaint6; }
            set { SetProperty(ref cameraRepaint6, value); }
        }
        private ObservableCollection<ROI> cameraROIList6 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList6
        {
            get { return cameraROIList6; }
            set { SetProperty(ref cameraROIList6, value); }
        }
        private HObject cameraAppendHObject6;
        public HObject CameraAppendHObject6
        {
            get { return cameraAppendHObject6; }
            set { SetProperty(ref cameraAppendHObject6, value); }
        }
        private HMsgEntry cameraAppendHMessage6;
        public HMsgEntry CameraAppendHMessage6
        {
            get { return cameraAppendHMessage6; }
            set { SetProperty(ref cameraAppendHMessage6, value); }
        }
        private Tuple<string, object> cameraGCStyle6;
        public Tuple<string, object> CameraGCStyle6
        {
            get { return cameraGCStyle6; }
            set { SetProperty(ref cameraGCStyle6, value); }
        }
        #endregion
        #region 7#
        private HImage cameraIamge7;
        public HImage CameraIamge7
        {
            get { return cameraIamge7; }
            set { SetProperty(ref cameraIamge7, value); }
        }
        private bool cameraRepaint7;
        public bool CameraRepaint7
        {
            get { return cameraRepaint7; }
            set { SetProperty(ref cameraRepaint7, value); }
        }
        private ObservableCollection<ROI> cameraROIList7 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList7
        {
            get { return cameraROIList7; }
            set { SetProperty(ref cameraROIList7, value); }
        }
        private HObject cameraAppendHObject7;
        public HObject CameraAppendHObject7
        {
            get { return cameraAppendHObject7; }
            set { SetProperty(ref cameraAppendHObject7, value); }
        }
        private HMsgEntry cameraAppendHMessage7;
        public HMsgEntry CameraAppendHMessage7
        {
            get { return cameraAppendHMessage7; }
            set { SetProperty(ref cameraAppendHMessage7, value); }
        }
        private Tuple<string, object> cameraGCStyle7;
        public Tuple<string, object> CameraGCStyle7
        {
            get { return cameraGCStyle7; }
            set { SetProperty(ref cameraGCStyle7, value); }
        }
        #endregion
        #region 8#
        private HImage cameraIamge8;
        public HImage CameraIamge8
        {
            get { return cameraIamge8; }
            set { SetProperty(ref cameraIamge8, value); }
        }
        private bool cameraRepaint8;
        public bool CameraRepaint8
        {
            get { return cameraRepaint8; }
            set { SetProperty(ref cameraRepaint8, value); }
        }
        private ObservableCollection<ROI> cameraROIList8 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList8
        {
            get { return cameraROIList8; }
            set { SetProperty(ref cameraROIList8, value); }
        }
        private HObject cameraAppendHObject8;
        public HObject CameraAppendHObject8
        {
            get { return cameraAppendHObject8; }
            set { SetProperty(ref cameraAppendHObject8, value); }
        }
        private HMsgEntry cameraAppendHMessage8;
        public HMsgEntry CameraAppendHMessage8
        {
            get { return cameraAppendHMessage8; }
            set { SetProperty(ref cameraAppendHMessage8, value); }
        }
        private Tuple<string, object> cameraGCStyle8;
        public Tuple<string, object> CameraGCStyle8
        {
            get { return cameraGCStyle8; }
            set { SetProperty(ref cameraGCStyle8, value); }
        }
        #endregion
        #endregion
        private string messageStr;
        public string MessageStr
        {
            get { return messageStr; }
            set { SetProperty(ref messageStr, value); }
        }
        private int stepNum;
        public int StepNum
        {
            get { return stepNum; }
            set { SetProperty(ref stepNum, value); }
        }
        #endregion
        #region 方法绑定
        private DelegateCommand testButtonCommand;
        public DelegateCommand TestButtonCommand =>
            testButtonCommand ?? (testButtonCommand = new DelegateCommand(ExecuteTestButtonCommand));
        private DelegateCommand reloadParamCommand;
        public DelegateCommand ReloadParamCommand =>
            reloadParamCommand ?? (reloadParamCommand = new DelegateCommand(ExecuteReloadParamCommand));

        void ExecuteReloadParamCommand()
        {
            var result = _dialogHostService.MessageBox(MessageType.Confirm, "信息", "确定重新加载下层参数吗？");
            if (result == ButtonResult.Yes)
            {
                LoadParam();
                LoadCameraParam();
                addMessage("重新加载参数");
            }
        }
        void ExecuteTestButtonCommand()
        {
            #region 位置比较触发
            //GTSCard.ComparePulseTrigger(Bottom_A8.A1.CardNo, 1);
            #endregion
            #region 回原点
            //GTSCard.AxisHomeMove(Bottom_A8.A2);
            #endregion
            #region 位置比较
            //GTSCard.ComparePulseTrigger(Bottom_A8.A1.CardNo, 2);
            #endregion
        }
        #endregion
        #region 构造函数
        void ImageCallbackFunc1(IntPtr pData, ref MV_FRAME_OUT_INFO_EX pFrameInfo, IntPtr pUser)
        {
            if (camDown.m_bIsDeviceOpen)
            {
                //Debug.WriteLine("Get one frame: Width[" + Convert.ToString(pFrameInfo.nWidth) + "] , Height[" + Convert.ToString(pFrameInfo.nHeight)
                //    + "] , FrameNum[" + Convert.ToString(pFrameInfo.nFrameNum) + "]");
                //Debug.WriteLine($"enPixelType:{pFrameInfo.enPixelType}");
                if (pFrameInfo.enPixelType == MvGvspPixelType.PixelType_Gvsp_Mono8)
                {
                    HObject Hobj;
                    HOperatorSet.GenImage1Extern(out Hobj, "byte", pFrameInfo.nWidth, pFrameInfo.nHeight, pData, IntPtr.Zero);
                    camDown.ImageQueue.Enqueue(Hobj.Clone());
                }
            }
        }
        void ImageCallbackFunc2(IntPtr pData, ref MV_FRAME_OUT_INFO_EX pFrameInfo, IntPtr pUser)
        {
            if (camUp.m_bIsDeviceOpen)
            {
                //Debug.WriteLine("Get one frame: Width[" + Convert.ToString(pFrameInfo.nWidth) + "] , Height[" + Convert.ToString(pFrameInfo.nHeight)
                //    + "] , FrameNum[" + Convert.ToString(pFrameInfo.nFrameNum) + "]");
                //Debug.WriteLine($"enPixelType:{pFrameInfo.enPixelType}");
                if (pFrameInfo.enPixelType == MvGvspPixelType.PixelType_Gvsp_Mono8)
                {
                    HObject Hobj;
                    HOperatorSet.GenImage1Extern(out Hobj, "byte", pFrameInfo.nWidth, pFrameInfo.nHeight, pData, IntPtr.Zero);
                    camUp.ImageQueue.Enqueue(Hobj.Clone());
                }
            }
        }
        public BottomFloorViewModel(IContainerProvider containerProvider) : base(containerProvider)
        {
            recognizeRegions = new HObject[4];
            ModelContourss = new HObject[4];
            ModelIDs = new HTuple[4];
            ShapeModelRows = new HTuple[4];
            ShapeModelColumns = new HTuple[4];
            ShapeModelAngles = new HTuple[4];
            StandardRows = new HTuple[4];
            StandardColumns = new HTuple[4];
            StandardAngles = new HTuple[4];
            roiLine1s = new ROILine[4];
            roiLine2s = new ROILine[4];
            MessageStr = string.Empty;
            suckerWorkArr = new SuckerWork[4];
            for (int i = 0; i < 4; i++)
            {
                suckerWorkArr[i] = new SuckerWork();
            }
            feederWorkArr = new FeederWork[4];
            for (int i = 0; i < 4; i++)
            {
                feederWorkArr[i] = new FeederWork();
            }
            paramService = containerProvider.Resolve<IParamService>("Param");
            Bottom_A8 = containerProvider.Resolve<IGTSCardService>("Bottom_A8");
            Top_A4 = containerProvider.Resolve<IGTSCardService>("Top_A4");
            plcModbus = containerProvider.Resolve<IPLCModbusService>("PLCModbus");
            var r1 = Bottom_A8.Init(8, "Bottom_A8", "Bottom_A8");
            addMessage($"下层8轴卡初始化:{(r1 ? "成功" : "失败")}");
            if (r1)
            {
                aggregator.SendMessage("BottomA8AxisCardOK", "Status");
            }
            else
            {
                aggregator.SendMessage("BottomA8AxisCardNG", "Status");
            }
            servoModbus = containerProvider.Resolve<IServoModbusService>("ServoModbus");
            camDown = containerProvider.Resolve<ICameraService>("Floor2Down");
            camDown.ImageQueue = new ConcurrentQueue<HObject>();
            camDown.ImageCallbackFunc = ImageCallbackFunc1;
            LoadParam();
            LoadCameraParam();
            var camr1 = camDown.OpenCamera("192.168.134.11");
            if (camr1)
            {
                camDown.SetExposureTime(downCameraExposureTime);
                aggregator.SendMessage("BottomDownCameraOK", "Status");
            }
            else
            {
                aggregator.SendMessage("BottomDownCameraNG", "Status");
            }
            camUp = containerProvider.Resolve<ICameraService>("Floor2Up");
            camUp.ImageQueue = new ConcurrentQueue<HObject>();
            camUp.ImageCallbackFunc = ImageCallbackFunc2;

            var camr2 = camUp.OpenCamera("192.168.133.11");
            if (camr2)
            {
                camUp.SetExposureTime(upCameraExposureTime);
                aggregator.SendMessage("BottomUpCameraOK", "Status");
            }
            else
            {
                aggregator.SendMessage("BottomUpCameraOK", "Status");
            }

            using (var sysDb = new SysContext())
            {
                var abspos = sysDb.MSystemParams.FirstOrDefault(p => p.Name == "BottomZ1");
                if (abspos != null)
                {
                    var absvalue = double.Parse(abspos.Value.ToString());
                    int pls = servoModbus.ReadInovance(13);
                    GTSCard.AxisPosSet(Bottom_A8.A4, pls * Bottom_A8.A4.Equiv - absvalue);
                    GTSCard.AxisEncSet(Bottom_A8.A4, pls * Bottom_A8.A4.Equiv - absvalue);
                }
                abspos = sysDb.MSystemParams.FirstOrDefault(p => p.Name == "BottomZ2");
                if (abspos != null)
                {
                    var absvalue = double.Parse(abspos.Value.ToString());
                    int pls = servoModbus.ReadInovance(14);
                    GTSCard.AxisPosSet(Bottom_A8.A5, pls * Bottom_A8.A5.Equiv - absvalue);
                    GTSCard.AxisEncSet(Bottom_A8.A5, pls * Bottom_A8.A5.Equiv - absvalue);
                }
                abspos = sysDb.MSystemParams.FirstOrDefault(p => p.Name == "BottomZ3");
                if (abspos != null)
                {
                    var absvalue = double.Parse(abspos.Value.ToString());
                    int pls = servoModbus.ReadInovance(15);
                    GTSCard.AxisPosSet(Bottom_A8.A6, pls * Bottom_A8.A6.Equiv - absvalue);
                    GTSCard.AxisEncSet(Bottom_A8.A6, pls * Bottom_A8.A6.Equiv - absvalue);
                }
                abspos = sysDb.MSystemParams.FirstOrDefault(p => p.Name == "BottomZ4");
                if (abspos != null)
                {
                    var absvalue = double.Parse(abspos.Value.ToString());
                    int pls = servoModbus.ReadInovance(16);
                    GTSCard.AxisPosSet(Bottom_A8.A7, pls * Bottom_A8.A7.Equiv - absvalue);
                    GTSCard.AxisEncSet(Bottom_A8.A7, pls * Bottom_A8.A7.Equiv - absvalue);
                }

                abspos = sysDb.MSystemParams.FirstOrDefault(p => p.Name == "BottomR1");
                if (abspos != null)
                {
                    var absvalue = double.Parse(abspos.Value.ToString());
                    int pls = servoModbus.ReadInovance(17);
                    GTSCard.AxisPosSet(Top_A4.A3, pls * Top_A4.A3.Equiv - absvalue);
                    GTSCard.AxisEncSet(Top_A4.A3, pls * Top_A4.A3.Equiv - absvalue);
                }
                abspos = sysDb.MSystemParams.FirstOrDefault(p => p.Name == "BottomR2");
                if (abspos != null)
                {
                    var absvalue = double.Parse(abspos.Value.ToString());
                    int pls = servoModbus.ReadInovance(18);
                    GTSCard.AxisPosSet(Top_A4.A4, pls * Top_A4.A4.Equiv - absvalue);
                    GTSCard.AxisEncSet(Top_A4.A4, pls * Top_A4.A4.Equiv - absvalue);
                }
                var _param = sysDb.MSystemParams.FirstOrDefault(p => p.Name == "Floor1DownCameraGrabPreTime");
                if (_param != null)
                {
                    downCameraGrabPreTime = double.Parse(_param.Value);
                }
                _param = sysDb.MSystemParams.FirstOrDefault(p => p.Name == "Floor1UpCameraGrabPreTime");
                if (_param != null)
                {
                    upCameraGrabPreTime = double.Parse(_param.Value);
                }
            }
            aggregator.ResgiterMessage(arg =>
            {
                switch (arg.Message)
                {
                    case "AppClose":
                        {
                            if (source != null)
                            {
                                source.Cancel();
                            }
                            if (source1 != null)
                            {
                                source1.Cancel();
                            }
                        }
                        break;
                    default:
                        break;
                }
            });
            aggregator.ResgiterMessage(arg => {
                switch (arg.Message)
                {
                    case "RailReady":
                        RailReadyFlag = true;
                        break;
                    case "AssistFinished":
                        AssistFinished = true;
                        break;
                    case "FirstStart":
                        startFlag = true;
                        break;
                    case "TopWordFinished":
                        TopWordFinished = true;
                        break;
                    default:
                        break;
                }
            }, "Run");
            source = new CancellationTokenSource();
            CancellationToken token = source.Token;
            Task.Run(() => Run(token), token);
        }
        #endregion
        #region 功能函数
        private void addMessage(string str)
        {
            //logger.Debug(str);
            Debug.WriteLine(str);
            string[] s = MessageStr.Split('\n');
            if (s.Length > 1000)
            {
                MessageStr = "";
            }
            if (MessageStr != "")
            {
                MessageStr += "\n";
            }
            MessageStr += DateTime.Now.ToString("yyyy/MM/dd HH:mm:ss") + " " + str;
        }
        private void LoadParam()
        {
            var partNum = paramService.GetPart();
            using (var db = new DTContext(partNum))
            {
                var param = db.MParams.FirstOrDefault(p => p.Name == "Z1SafePos" && p.Floor == 2);
                if (param != null)
                {
                    z1SafePos = double.Parse(param.Value);
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "Z2SafePos" && p.Floor == 2);
                if (param != null)
                {
                    z2SafePos = double.Parse(param.Value);
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "Z3SafePos" && p.Floor == 2);
                if (param != null)
                {
                    z3SafePos = double.Parse(param.Value);
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "Z4SafePos" && p.Floor == 2);
                if (param != null)
                {
                    z4SafePos = double.Parse(param.Value);
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "XRunSpeed" && p.Floor == 2);
                if (param != null)
                {
                    xRunSpeed = double.Parse(param.Value);
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "YRunSpeed" && p.Floor == 2);
                if (param != null)
                {
                    yRunSpeed = double.Parse(param.Value);
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "ZRunSpeed" && p.Floor == 2);
                if (param != null)
                {
                    zRunSpeed = double.Parse(param.Value);
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "RRunSpeed" && p.Floor == 2);
                if (param != null)
                {
                    rRunSpeed = double.Parse(param.Value);
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "IsSimulate");
                if (param != null)
                {
                    IsSimulate = param.Value == "1";
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "XYGapLimit");
                if (param != null)
                {
                    if (double.TryParse(param.Value, out var v1))
                    {
                        xYGapLimit = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "RGapLimit");
                if (param != null)
                {
                    if (double.TryParse(param.Value, out var v1))
                    {
                        rGapLimit = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "UpCameraExposureTime" && p.Floor == 2);
                if (param != null)
                {
                    if (float.TryParse(param.Value, out var v1))
                    {
                        upCameraExposureTime = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "DownCameraExposureTime" && p.Floor == 2);
                if (param != null)
                {
                    if (float.TryParse(param.Value, out var v1))
                    {
                        downCameraExposureTime = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "FilmSuckTime" && p.Floor == 2);
                if (param != null)
                {
                    if (double.TryParse(param.Value, out var v1))
                    {
                        filmSuckTime = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "FilmReleaseTime" && p.Floor == 2);
                if (param != null)
                {
                    if (double.TryParse(param.Value, out var v1))
                    {
                        filmReleaseTime = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "ZSlowDistSuck" && p.Floor == 2);
                if (param != null)
                {
                    if (double.TryParse(param.Value, out var v1))
                    {
                        zSlowDistSuck = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "ZSlowDistPaste" && p.Floor == 2);
                if (param != null)
                {
                    if (double.TryParse(param.Value, out var v1))
                    {
                        zSlowDistPaste = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "ZSlowSpeedSuck" && p.Floor == 2);
                if (param != null)
                {
                    if (double.TryParse(param.Value, out var v1))
                    {
                        zSlowSpeedSuck = v1;
                    }
                }
                param = db.MParams.FirstOrDefault(p => p.Name == "ZSlowSpeedPaste" && p.Floor == 2);
                if (param != null)
                {
                    if (double.TryParse(param.Value, out var v1))
                    {
                        zSlowSpeedPaste = v1;
                    }
                }
                initPoint = db.XYZRPoints.FirstOrDefault(p => p.Name == "Init" && p.Floor == 2);
                suckPicker1 = db.XYZRPoints.FirstOrDefault(p => p.Name == "SuckPicker1" && p.Floor == 2);
                suckPicker2 = db.XYZRPoints.FirstOrDefault(p => p.Name == "SuckPicker2" && p.Floor == 2);
                suckPicker3 = db.XYZRPoints.FirstOrDefault(p => p.Name == "SuckPicker3" && p.Floor == 2);
                suckPicker4 = db.XYZRPoints.FirstOrDefault(p => p.Name == "SuckPicker4" && p.Floor == 2);
                flyStartPicker = db.XYZRPoints.FirstOrDefault(p => p.Name == "FlyStartPicker" && p.Floor == 2);
                flyEndPicker = db.XYZRPoints.FirstOrDefault(p => p.Name == "FlyEndPicker" && p.Floor == 2);
                buttomCameraGrab_Sucker1 = db.XYZRPoints.FirstOrDefault(p => p.Name == "ButtomCameraGrab_Sucker1" && p.Floor == 2);
                centerSuck2 = db.XYZRPoints.FirstOrDefault(p => p.Name == "CenterSuck2" && p.Floor == 2);
                centerSuck3 = db.XYZRPoints.FirstOrDefault(p => p.Name == "CenterSuck3" && p.Floor == 2);
                centerSuck4 = db.XYZRPoints.FirstOrDefault(p => p.Name == "CenterSuck4" && p.Floor == 2);
                calibpoint = db.XYZRPoints.FirstOrDefault(p => p.Name == "TopCameraCalib" && p.Floor == 2);
                bottomCameraFail_Sucker1 = db.XYZRPoints.FirstOrDefault(p => p.Name == "BottomCameraFail_Sucker1" && p.Floor == 2);
                topCameraMark1 = db.XYZRPoints.FirstOrDefault(p => p.Name == "TopCameraMark1" && p.Floor == 2);
                topCameraMark2 = db.XYZRPoints.FirstOrDefault(p => p.Name == "TopCameraMark2" && p.Floor == 2);

                for (int i = 0; i < 4; i++)
                {
                    param = db.MParams.FirstOrDefault(p => p.Name == $"Feeder{i + 1}" && p.Floor == 2);
                    if (param != null)
                    {
                        var _feeder = JsonConvert.DeserializeObject<Feeder>(param.Value);
                        feederWorkArr[i].ColumnCount = _feeder.ColumnCount;
                        feederWorkArr[i].DeltaX = _feeder.deltaX;
                        feederWorkArr[i].DeltaY = _feeder.deltaY;
                    }
                }
                for (int i = 0; i < 4; i++)
                {
                    param = db.MParams.FirstOrDefault(p => p.Name == $"Sucker{i + 1}" && p.Floor == 2);
                    if (param != null)
                    {
                        var _sucker = JsonConvert.DeserializeObject<Sucker>(param.Value);
                        suckerWorkArr[i].FeederIndex = _sucker.FeederIndex;
                    }
                }
                   
            }
        }
        private void LoadCameraParam()
        {
            var partNum = paramService.GetPart();
            try
            {
                #region 拍产品相机
                string upfilepath = $"{partNum}\\Camera\\Bottom\\Calc\\Up";
                string upcalibfilepath = $"Camera\\Bottom\\Calib\\Up";
                HOperatorSet.ReadRegion(out Mark1Region, System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "Mark1Region.hobj"));
                HOperatorSet.ReadRegion(out Mark2Region, System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "Mark2Region.hobj"));
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, upcalibfilepath, "CalibHomMat2D.tup"), out UpCameraCalibHomMat2D);
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "Mark1Row.tup"), out Mark1Row);
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "Mark1Column.tup"), out Mark1Column);
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "Mark2Row.tup"), out Mark2Row);
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "Mark2Column.tup"), out Mark2Column);
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "MarkMidX.tup"), out MarkMidX0);
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "MarkMidY.tup"), out MarkMidY0);
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, upfilepath, "MarkMidR.tup"), out MarkMidR0);
                using var db = new DTContext(partNum);
                var v1 = db.MParams.FirstOrDefault(p => p.Name == "Mark1ThresholdMaxGray" && p.Floor == 2);
                if (v1 != null)
                {
                    Mark1ThresholdMaxGray = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Mark1MinArea" && p.Floor == 2);
                if (v1 != null)
                {
                    Mark1MinArea = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Mark1MaxArea" && p.Floor == 2);
                if (v1 != null)
                {
                    Mark1MaxArea = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Mark2ThresholdMaxGray" && p.Floor == 2);
                if (v1 != null)
                {
                    Mark2ThresholdMaxGray = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Mark2MinArea" && p.Floor == 2);
                if (v1 != null)
                {
                    Mark2MinArea = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Mark2MaxArea" && p.Floor == 2);
                if (v1 != null)
                {
                    Mark2MaxArea = double.Parse(v1.Value);
                }
                #endregion
                #region 拍膜相机

                string downcalibfilepath = $"Camera\\Bottom\\Calib\\Down";
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, downcalibfilepath, "CalibHomMat2D.tup"), out CalibHomMat2D);
                for (int i = 0; i < 4; i++)
                {
                    string downfilepath = $"{partNum}\\Camera\\Bottom\\Calc\\Down\\{i + 1}";
                    HOperatorSet.ReadRegion(out var recognizeRegion, System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "RecognizeRegion.hobj"));
                    recognizeRegions[i] = recognizeRegion;
                    HOperatorSet.ReadShapeModel(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "ShapeModelID.shm"), out var ModelID);
                    ModelIDs[i] = ModelID;
                    HOperatorSet.ReadContourXldDxf(out var ModelContours, System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "ShapeModelContours.dxf"), new HTuple(), new HTuple(), out var hv_DxfStatus);
                    ModelContourss[i] = ModelContours;
                    HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "ShapeModelRow.tup"), out var ShapeModelRow);
                    ShapeModelRows[i] = ShapeModelRow;
                    HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "ShapeModelColumn.tup"), out var ShapeModelColumn);
                    ShapeModelColumns[i] = ShapeModelColumn;
                    HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "ShapeModelAngle.tup"), out var ShapeModelAngle);
                    ShapeModelAngles[i] = ShapeModelAngle;
                    string jsonString = File.ReadAllText(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "LineRegion1.json"));
                    roiLine1s[i] = JsonConvert.DeserializeObject<ROILine>(jsonString);
                    jsonString = File.ReadAllText(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "LineRegion2.json"));
                    roiLine2s[i] = JsonConvert.DeserializeObject<ROILine>(jsonString);
                    HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "StandardRow.tup"), out var StandardRow);
                    StandardRows[i] = StandardRow;
                    HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "StandardColumn.tup"), out var StandardColumn);
                    StandardColumns[i] = StandardColumn;
                    HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, downfilepath, "StandardAngle.tup"), out var StandardAngle);
                    StandardAngles[i] = StandardAngle;
                }
                #endregion
            }
            catch (Exception ex)
            {
                addMessage(ex.Message);
            }
        }
        private void Run(CancellationToken token)
        {
            #region 变量
            int stepnum = 0;
            bool filmCCDCalcFinish = false;
            int CameraIndex = 0;

            double flySpeed = 0;

            var partNum = paramService.GetPart();

            bool XAlarm = false, YAlarm = false, Z1Alarm = false, Z2Alarm = false, Z3Alarm = false, Z4Alarm = false, R1Alarm = false, R2Alarm = false;
            bool EMGButton = true, ResetButton = false, PauseButton = false, StartButton = false, SafeGrate = false;
            Stopwatch sw = new Stopwatch();
            var axisX = Bottom_A8.A1; var axisY = Bottom_A8.A2; var axisZ1 = Bottom_A8.A4; var axisZ2 = Bottom_A8.A5; var axisZ3 = Bottom_A8.A6; var axisZ4 = Bottom_A8.A7;
            var axisR1 = Top_A4.A3; var axisR2 = Top_A4.A4;
            double zSafePos = 0;
            AxisParm axisZ = default, axisR = default;
            double suckCenterX = 0, suckCenterY = 0;
            bool XHomeFinishFlag = false, YHomeFinishFlag = false;

            Queue<ProductFly> productQueue = new Queue<ProductFly>();
            ProductFly _CurPoint = new ProductFly();
            List<ImageProductFly> imageProductFlyList = new List<ImageProductFly>();

            List<MPcsWork> pickerPcsList = new List<MPcsWork>();
            Queue<MPcsWork> suckPcsQueue = new Queue<MPcsWork>();
            Queue<MPcsWork> releaseFailPcsQueue = new Queue<MPcsWork>();
            Queue<MPcsWork> pastePcsQueue = new Queue<MPcsWork>();
            Queue<MPcsWork> pcsQueue = new Queue<MPcsWork>();

            MPcsWork _curSuckPcs = new MPcsWork(); MPcsWork _curPastePcs = new MPcsWork(); MPcsWork _curReleasePcs = default;

            XYZRPoint suckPicker = new XYZRPoint();

            bool isProductFlyGrabed = false;
            bool mark1GabFinished = false, mark2GabFinished = false, mark1CalcFinished = false, mark2CalcFinished = false;
            MMarkResult markResult1 = default;
            HTuple UpCameraHomMat2D = null, UpCameraDeltaAngle = null;
            HObject image5, image6;
            HOperatorSet.GenEmptyObj(out image5);
            HOperatorSet.GenEmptyObj(out image6);
            int cycleCount = 0;
            #endregion
            System.Threading.Thread.Sleep(2500);//开机延时2.5秒
            addMessage("请按\"复位\"按钮");
            while (true)
            {
                if (token.IsCancellationRequested)
                {
                    return;
                }
                try
                {
                    #region IO信号
                    var eMGButton = plcModbus.XCoils_X0[3] && plcModbus.XCoils_X0[4] && plcModbus.XCoils_X0[8] && plcModbus.XCoils_X0[9];
                    StartButton = plcModbus.XCoils_X0[0];
                    var resetButton = plcModbus.XCoils_X0[1];
                    var pauseButton = plcModbus.XCoils_X0[2];
                    var safeGrate = plcModbus.XCoils_X0[10];
                    var xAlarm = GTSCard.GetAxisAlarm(axisX);
                    var yAlarm = GTSCard.GetAxisAlarm(axisY);
                    var z1Alarm = GTSCard.GetAxisAlarm(axisZ1);
                    var z2Alarm = GTSCard.GetAxisAlarm(axisZ2);
                    var z3Alarm = GTSCard.GetAxisAlarm(axisZ3);
                    var z4Alarm = GTSCard.GetAxisAlarm(axisZ4);
                    var r1Alarm = GTSCard.GetAxisAlarm(axisR1);
                    var r2Alarm = GTSCard.GetAxisAlarm(axisR2);
                    if (EMGButton != eMGButton)
                    {
                        if (!eMGButton)
                        {
                            addMessage("急停按钮按下");
                        }
                        EMGButton = eMGButton;
                    }
                    if (PauseButton != pauseButton)
                    {
                        if (!pauseButton)
                        {
                            addMessage("停止按钮按下");
                            aggregator.SendMessage("下层:停止按钮按下", "Alarm");
                        }
                        PauseButton = pauseButton;
                    }
                    if (SafeGrate != safeGrate)
                    {
                        if (!safeGrate)
                        {
                            addMessage("光栅被挡");
                        }
                        SafeGrate = safeGrate;
                    }
                    if (XAlarm != xAlarm)
                    {
                        if (xAlarm)
                        {
                            addMessage("X轴报警");
                            aggregator.SendMessage("下层:X轴报警", "Alarm");
                        }
                        XAlarm = xAlarm;
                    }
                    if (YAlarm != yAlarm)
                    {
                        if (yAlarm)
                        {
                            addMessage("Y轴报警");
                            aggregator.SendMessage("下层:Y轴报警", "Alarm");
                        }
                        YAlarm = yAlarm;
                    }
                    if (Z1Alarm != z1Alarm)
                    {
                        if (z1Alarm)
                        {
                            addMessage("Z1轴报警");
                            aggregator.SendMessage("下层:Z1轴报警", "Alarm");
                        }
                        Z1Alarm = z1Alarm;
                    }
                    if (Z2Alarm != z2Alarm)
                    {
                        if (z2Alarm)
                        {
                            addMessage("Z2轴报警");
                            aggregator.SendMessage("下层:Z2轴报警", "Alarm");
                        }
                        Z2Alarm = z2Alarm;
                    }
                    if (Z3Alarm != z3Alarm)
                    {
                        if (z3Alarm)
                        {
                            addMessage("Z3轴报警");
                            aggregator.SendMessage("下层:Z3轴报警", "Alarm");
                        }
                        Z3Alarm = z3Alarm;
                    }
                    if (Z4Alarm != z4Alarm)
                    {
                        if (z4Alarm)
                        {
                            addMessage("Z4轴报警");
                            aggregator.SendMessage("下层:Z4轴报警", "Alarm");
                        }
                        Z4Alarm = z4Alarm;
                    }
                    if (R1Alarm != r1Alarm)
                    {
                        if (r1Alarm)
                        {
                            addMessage("R1轴报警");
                            aggregator.SendMessage("下层:R1轴报警", "Alarm");
                        }
                        R1Alarm = r1Alarm;
                    }
                    if (R2Alarm != r2Alarm)
                    {
                        if (r2Alarm)
                        {
                            addMessage("R2轴报警");
                            aggregator.SendMessage("下层:R2轴报警", "Alarm");
                        }
                        R2Alarm = r2Alarm;
                    }
                    if (!pauseButton || !safeGrate || xAlarm || yAlarm || z1Alarm || z2Alarm || z3Alarm || z4Alarm || r1Alarm || r2Alarm)
                    {
                        PauseFlag = true;
                    }
                    else
                    {
                        if (PauseFlag)
                        {
                            if (resetButton)
                            {
                                PauseFlag = false;
                            }
                        }
                    }
                    #endregion
                    #region 外循环
                    if (!EMGButton && stepnum >= 0)
                    {
                        if (source1 != null)
                        {
                            source1.Cancel();
                        }
                        pickerPcsList.Clear();
                        suckPcsQueue.Clear();
                        releaseFailPcsQueue.Clear();
                        pastePcsQueue.Clear();
                        pcsQueue.Clear();
                        ResetAction();
                        RailReadyFlag = false;
                        AssistFinished = false;
                        TopWordFinished = false;
                        isProductFlyGrabed = false;
                        startFlag = false;
                        stepnum = -2;
                    }

                    #endregion
                    #region 流程
                    switch (stepnum)
                    {
                        #region 急停
                        case -2:
                            if (EMGButton)
                            {
                                addMessage("请按\"复位\"按钮");
                                stepnum = 0;
                            }
                            break;
                        #endregion
                        #region 复位
                        case 0:
                            if (ResetButton != resetButton)
                            {
                                if (resetButton)
                                {
                                    partNum = paramService.GetPart();
                                    stepnum = 1;
                                }
                                ResetButton = resetButton;
                            }
                            break;
                        case 1:
                            if (!PauseFlag)
                            {
                                //励磁                           
                                GTSCard.ServoOn(axisZ1);
                                GTSCard.ServoOn(axisZ2);
                                GTSCard.ServoOn(axisZ3);
                                GTSCard.ServoOn(axisZ4);
                                GTSCard.ServoOn(axisR1);
                                GTSCard.ServoOn(axisR2);
                                GTSCard.SetDo(Bottom_A8.A1.CardNo, 0, 0);
                                GTSCard.SetDo(Bottom_A8.A1.CardNo, 1, 0);
                                GTSCard.SetDo(Bottom_A8.A1.CardNo, 2, 0);
                                GTSCard.SetDo(Bottom_A8.A1.CardNo, 3, 0);
                                sw.Restart();
                                stepnum = 2;
                            }
                            break;
                        case 2:
                            if (!PauseFlag)
                            {
                                if (sw.Elapsed.TotalSeconds > 2)
                                {
                                    sw.Stop();
                                    //把编码器值复制到规划值
                                    GTSCard.AxisPosSet(axisX, GTSCard.GetEnc(axisX));
                                    GTSCard.AxisPosSet(axisY, GTSCard.GetEnc(axisY));
                                    GTSCard.AxisPosSet(axisZ1, GTSCard.GetEnc(axisZ1));
                                    GTSCard.AxisPosSet(axisZ2, GTSCard.GetEnc(axisZ2));
                                    GTSCard.AxisPosSet(axisZ3, GTSCard.GetEnc(axisZ3));
                                    GTSCard.AxisPosSet(axisZ4, GTSCard.GetEnc(axisZ4));
                                    GTSCard.AxisPosSet(axisR1, GTSCard.GetEnc(axisR1));
                                    GTSCard.AxisPosSet(axisR2, GTSCard.GetEnc(axisR2));
                                    stepnum = 3;
                                }
                            }
                            break;
                        case 3:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ1, z1SafePos, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ2, z2SafePos, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ3, z3SafePos, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ4, z4SafePos, zRunSpeed);
                                stepnum = 4;
                            }
                            break;
                        case 4:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ1) && GTSCard.AxisPosMoveCheckDone(axisZ2) && GTSCard.AxisPosMoveCheckDone(axisZ3) && GTSCard.AxisPosMoveCheckDone(axisZ4))
                                {
                                    GTSCard.ServoOn(axisX);
                                    GTSCard.ServoOn(axisY);
                                    sw.Restart();
                                    stepnum = 5;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ1, 0);
                                GTSCard.AxisStop(axisZ2, 0);
                                GTSCard.AxisStop(axisZ3, 0);
                                GTSCard.AxisStop(axisZ4, 0);
                                stepnum = 3;
                            }
                            break;
                        case 5://Y轴复位和X轴回原点
                            if (!PauseFlag)
                            {
                                if (sw.Elapsed.TotalSeconds > 2)
                                {
                                    sw.Stop();
                                    source1 = new CancellationTokenSource();
                                    CancellationToken token1 = source1.Token;
                                    XHomeFinishFlag = false;
                                    Task.Run(() => XHome(token1), token1).ContinueWith(t => XHomeFinishFlag = true);
                                    YHomeFinishFlag = false;
                                    Task.Run(() => YHome(token1), token1).ContinueWith(t => YHomeFinishFlag = true);
                                    stepnum = 6;
                                }

                            }
                            break;
                        case 6:
                            if (!PauseFlag)
                            {
                                if (XHomeFinishFlag && YHomeFinishFlag)
                                {
                                    stepnum = 7;
                                }
                            }
                            break;
                        case 7:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, initPoint.X, xRunSpeed / 5);
                                GTSCard.AxisPosMove(ref axisY, initPoint.Y, yRunSpeed / 5);
                                GTSCard.AxisPosMove(ref axisZ1, initPoint.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ2, initPoint.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ3, initPoint.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ4, initPoint.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisR1, initPoint.R, rRunSpeed);
                                GTSCard.AxisPosMove(ref axisR2, initPoint.R, rRunSpeed);
                                stepnum = 8;
                            }
                            break;
                        case 8:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY)
                                    && GTSCard.AxisPosMoveCheckDone(axisZ1) && GTSCard.AxisPosMoveCheckDone(axisZ2) && GTSCard.AxisPosMoveCheckDone(axisZ3) && GTSCard.AxisPosMoveCheckDone(axisZ4)
                                    && GTSCard.AxisPosMoveCheckDone(axisR1) && GTSCard.AxisPosMoveCheckDone(axisR2))
                                {
                                    addMessage("复位完成");
                                    stepnum = 9;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisZ1, 0);
                                GTSCard.AxisStop(axisZ2, 0);
                                GTSCard.AxisStop(axisZ3, 0);
                                GTSCard.AxisStop(axisZ4, 0);
                                GTSCard.AxisStop(axisR1, 0);
                                GTSCard.AxisStop(axisR2, 0);
                                stepnum = 7;
                            }
                            break;
                        #endregion
                        #region 启动
                        case 9:
                            if (!PauseFlag)
                            {
                                if (startFlag)
                                {
                                    stepnum = 1000;//取膜
                                }
                            }
                            break;
                        #endregion
                        #region 上相机拍Mark
                        case 10:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, topCameraMark1.X, xRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, topCameraMark1.Y, yRunSpeed);
                                stepnum = 11011;
                            }
                            break;
                        case 11011:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY))
                                {
                                    stepnum = 11;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                stepnum = 10;
                            }
                            break;
                        case 11:
                            if (!PauseFlag)
                            {
                                if (RailReadyFlag)
                                {
                                    camUp.ImageQueue.Clear();
                                    camUp.SetTriggerMode(0);
                                    camUp.StartGrabe();
                                    mark1GabFinished = false;
                                    Task.Run(() => {
                                        int mIndex = 0;
                                        while (mIndex < 1)
                                        {
                                            HObject? hObj;
                                            if (camUp.ImageQueue.TryPeek(out hObj))
                                            {
                                                image5.Dispose();
                                                camUp.ImageQueue.TryDequeue(out image5);
                                                mIndex++;
                                            }
                                        }
                                    }).ContinueWith(t => { camUp.StopGrabe(); mark1GabFinished = true; });
                                    stepnum = 12;
                                }
                            }
                            break;
                        case 12:
                            if (!PauseFlag)
                            {
                                if (mark1GabFinished)
                                {
                                    stepnum = 13;
                                    //处理图像1
                                    markResult1 = default;
                                    mark1CalcFinished = false;
                                    Task.Run(() => {
                                        try
                                        {
                                            HOperatorSet.ReduceDomain(image5, Mark1Region, out var imageReduced);
                                            ImageCalc.MFindAreaCenter(imageReduced, out var ho_resultRegion, Mark1ThresholdMaxGray, Mark1MinArea, Mark1MaxArea, out var hv_result, out var hv_row, out var hv_column);
                                            if (hv_result.I == 1)
                                            {
                                                markResult1.Result = true;
                                                markResult1.Row = hv_row.D;
                                                markResult1.Column = hv_column.D;
                                                addMessage("Mark1识别成功。");
                                            }
                                            else
                                            {
                                                addMessage("Mark1识别失败。");
                                            }
                                            System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                            {
                                                try
                                                {
                                                    CameraIamge5 = new HImage(image5);
                                                    if (hv_result.I == 1)
                                                    {
                                                        CameraGCStyle5 = new Tuple<string, object>("DrawMode", "fill");
                                                        CameraGCStyle5 = new Tuple<string, object>("Color", "magenta");
                                                        CameraAppendHObject5 = ho_resultRegion;
                                                        CameraAppendHMessage5 = new HMsgEntry($"Row:{hv_row.D:f1}", 10, 10, "green", "window", "box", "false", 32, "mono", "true", "false");
                                                        CameraAppendHMessage5 = new HMsgEntry($"Column:{hv_column.D:f1}", 40, 10, "green", "window", "box", "false", 32, "mono", "true", "false");
                                                    }
                                                    else
                                                    {
                                                        CameraAppendHMessage5 = new HMsgEntry("识别失败。", 10, 10, "red", "window", "box", "false", 32, "mono", "true", "false");
                                                    }
                                                }
                                                catch { }
                                            }));
                                        }
                                        catch (Exception ex)
                                        {
                                            addMessage(ex.Message);
                                        }
                                    }).ContinueWith(t => { mark1CalcFinished = true; });
                                }
                            }
                            break;
                        case 13:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, topCameraMark2.X, xRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, topCameraMark2.Y, yRunSpeed);
                                stepnum = 14;
                            }
                            break;
                        case 14:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY))
                                {
                                    camUp.ImageQueue.Clear();
                                    camUp.SetTriggerMode(0);
                                    camUp.StartGrabe();
                                    mark2GabFinished = false;
                                    Task.Run(() => {
                                        int mIndex = 0;
                                        while (mIndex < 1)
                                        {
                                            HObject? hObj;
                                            if (camUp.ImageQueue.TryPeek(out hObj))
                                            {
                                                image6.Dispose();
                                                camUp.ImageQueue.TryDequeue(out image6);
                                                mIndex++;
                                            }
                                        }
                                    }).ContinueWith(t => { camUp.StopGrabe(); mark2GabFinished = true; });
                                    stepnum = 15;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                stepnum = 13;
                            }
                            break;
                        case 15:
                            if (!PauseFlag)
                            {
                                if (mark2GabFinished && mark1CalcFinished)
                                {
                                    //产品拍完成
                                    isProductFlyGrabed = true;
                                    stepnum = 1050;//去贴料
                                    //处理图像2
                                    mark2CalcFinished = false;
                                    Task.Run(() => {
                                        try
                                        {
                                            double mark1Row, mark1Column;
                                            if (markResult1.Result)
                                            {
                                                mark1Row = markResult1.Row;
                                                mark1Column = markResult1.Column;
                                            }
                                            else
                                            {
                                                mark1Row = Mark1Row.D;
                                                mark1Column = Mark1Column.D;
                                            }
                                            HOperatorSet.AffineTransPoint2d(UpCameraCalibHomMat2D, mark1Row, mark1Column, out var mark1x, out var mark1y);

                                            var mark1x1 = topCameraMark1.X - calibpoint.X + mark1x;
                                            var mark1y1 = topCameraMark1.Y - calibpoint.Y + mark1y;

                                            HOperatorSet.ReduceDomain(image6, Mark2Region, out var imageReduced);
                                            ImageCalc.MFindAreaCenter(imageReduced, out var ho_resultRegion, Mark2ThresholdMaxGray, Mark2MinArea, Mark2MaxArea, out var hv_result, out var hv_row, out var hv_column);

                                            double mark2Row, mark2Column;
                                            if (hv_result.I == 1)
                                            {
                                                mark2Row = hv_row.D;
                                                mark2Column = hv_column.D;
                                                addMessage("Mark2识别成功。");
                                            }
                                            else
                                            {
                                                addMessage("Mark2识别失败。");
                                                mark2Row = Mark2Row.D;
                                                mark2Column = Mark2Column.D;
                                            }

                                            HOperatorSet.AffineTransPoint2d(UpCameraCalibHomMat2D, mark2Row, mark2Column, out var mark2x, out var mark2y);
                                            var mark2x1 = topCameraMark2.X - calibpoint.X + mark2x;
                                            var mark2y1 = topCameraMark2.Y - calibpoint.Y + mark2y;

                                            var markMidx = (mark1x1 + mark2x1) / 2;
                                            var markMidy = (mark1y1 + mark2y1) / 2;
                                            var hv_Angle = new HTuple(Math.Atan2(mark2y1 - mark1y1, mark2x1 - mark1x1));

                                            HOperatorSet.VectorAngleToRigid(MarkMidX0, MarkMidY0, MarkMidR0, markMidx, markMidy, hv_Angle, out UpCameraHomMat2D);
                                            UpCameraDeltaAngle = hv_Angle - MarkMidR0;
                                            var deltax = markMidx - MarkMidX0;
                                            var deltay = markMidy - MarkMidY0;
                                            var deltar = hv_Angle - MarkMidR0;
                                            addMessage($"载具:ΔX{deltax.D:f3}mm,ΔY{deltay.D:f3}mm,ΔR{deltar.TupleDeg().D:f2}°");
                                            System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                            {
                                                try
                                                {
                                                    CameraIamge6 = new HImage(image6);
                                                    if (hv_result.I == 1)
                                                    {
                                                        CameraGCStyle6 = new Tuple<string, object>("DrawMode", "fill");
                                                        CameraGCStyle6 = new Tuple<string, object>("Color", "magenta");
                                                        CameraAppendHObject6 = ho_resultRegion;
                                                        CameraAppendHMessage6 = new HMsgEntry($"Row:{hv_row.D:f1}", 10, 10, "green", "window", "box", "false", 32, "mono", "true", "false");
                                                        CameraAppendHMessage6 = new HMsgEntry($"Column:{hv_column.D:f1}", 40, 10, "green", "window", "box", "false", 32, "mono", "true", "false");
                                                    }
                                                    else
                                                    {
                                                        CameraAppendHMessage6 = new HMsgEntry("识别失败。", 10, 10, "red", "window", "box", "false", 32, "mono", "true", "false");
                                                    }
                                                }
                                                catch { }
                                            }));
                                        }
                                        catch (Exception ex)
                                        {
                                            addMessage(ex.Message);
                                        }
                                    }).ContinueWith(t => { mark2CalcFinished = true; });
                                }
                            }
                            break;
                        #endregion
                        #region 取膜
                        case 1000:
                            {
                                pcsQueue.Clear();
                                pickerPcsList.Clear();
                                using (var db = new DTContext(partNum))
                                {
                                    var _products = db.MProducts.Where(product => product.Floor == 2 && product.IsUse).ToList();
                                    if (_products.Any())
                                    {
                                        foreach (var product in _products)
                                        {
                                            var _pcses = product.Pcses.ToList();
                                            if (_pcses.Any())
                                            {
                                                foreach (var pcs in _pcses)
                                                {
                                                    pcsQueue.Enqueue(new MPcsWork
                                                    {
                                                        PcsOrder = product.PcsOrder,
                                                        PickIndex = pcs.PickIndex,
                                                        PressIndex = pcs.PressIndex,
                                                        PcsIndex = pcs.Index,
                                                        X = pcs.X,
                                                        Y = pcs.Y,
                                                        Z = pcs.Z,
                                                        R = pcs.R,
                                                        X1 = pcs.X1,
                                                        Y1 = pcs.Y1,
                                                        Z1 = pcs.Z1,
                                                        R1 = pcs.R1,
                                                        FilmX = -999,
                                                        FilmY = -999,
                                                        FilmR = -999
                                                    });
                                                }
                                            }
                                        }
                                    }
                                }
                                stepnum = 1001;
                            }
                            break;
                        case 1001:
                            {
                                pickerPcsList.Clear();
                                //装在工作数组
                                for (int i = 0; i < 4; i++)//4个头
                                {
                                    if (pcsQueue.TryPeek(out var _pcs))
                                    {
                                        var _pickerPcs = pickerPcsList.FirstOrDefault(p => p.PickIndex == _pcs.PickIndex);
                                        if (_pickerPcs == null)
                                        {
                                            pickerPcsList.Add(pcsQueue.Dequeue());
                                        }
                                        else
                                        {
                                            //吸头重复
                                            break;
                                        }
                                    }
                                    else
                                    {
                                        //队列已空
                                        break;
                                    }
                                }
                                if (pickerPcsList.Any())
                                {
                                    //有内容，可以工作
                                    pickerPcsList.Sort((p1, p2) => p1.PickIndex.CompareTo(p2.PickIndex));//按吸头顺序排序


                                    foreach (var pcs in pickerPcsList)
                                    {
                                        suckPcsQueue.Enqueue(pcs);
                                    }
                                    stepnum = 1002;//去取料
                                }
                                else
                                {
                                    stepnum = 1200;
                                    //没有合适内容→结束
                                }
                            }
                            break;
                        case 1002:
                            if (suckPcsQueue.TryPeek(out var suckPcs))
                            {
                                _curSuckPcs = suckPcsQueue.Dequeue();
                                stepnum = 1011;
                            }
                            else
                            {
                                stepnum = 1030;//去飞拍
                            }
                            break;
                        case 1011:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ1, z1SafePos, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ2, z2SafePos, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ3, z3SafePos, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ4, z4SafePos, zRunSpeed);
                                stepnum = 1012;
                            }
                            break;
                        case 1012:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ1) && GTSCard.AxisPosMoveCheckDone(axisZ2) && GTSCard.AxisPosMoveCheckDone(axisZ3) && GTSCard.AxisPosMoveCheckDone(axisZ4))
                                {
                                    stepnum = 1013;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ1, 0);
                                GTSCard.AxisStop(axisZ2, 0);
                                GTSCard.AxisStop(axisZ3, 0);
                                GTSCard.AxisStop(axisZ4, 0);
                                stepnum = 1011;
                            }
                            break;
                        case 1013:
                            if (!PauseFlag)
                            {
                                var counter = feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].Counter;
                                var deltaX = feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].DeltaX * counter;
                                var deltaY = feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].DeltaY * counter;

                                switch (suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex)
                                {
                                    case 0:
                                        suckPicker = suckPicker1;
                                        break;
                                    case 1:
                                        suckPicker = suckPicker2;
                                        break;
                                    case 2:
                                        suckPicker = suckPicker3;
                                        break;
                                    case 3:
                                    default:
                                        suckPicker = suckPicker4;
                                        break;
                                }

                                switch (_curSuckPcs.PickIndex)
                                {
                                    case 0:
                                        axisZ = axisZ1;
                                        axisR = axisR1;
                                        suckCenterX = 0;
                                        suckCenterY = 0;
                                        zSafePos = z1SafePos;
                                        break;
                                    case 1:
                                        axisZ = axisZ2;
                                        axisR = axisR1;
                                        suckCenterX = centerSuck2.X;
                                        suckCenterY = centerSuck2.Y;
                                        zSafePos = z2SafePos;
                                        break;
                                    case 2:
                                        axisZ = axisZ3;
                                        axisR = axisR2;
                                        suckCenterX = centerSuck3.X;
                                        suckCenterY = centerSuck3.Y;
                                        zSafePos = z3SafePos;
                                        break;
                                    case 3:
                                    default:
                                        axisZ = axisZ4;
                                        axisR = axisR2;
                                        suckCenterX = centerSuck4.X;
                                        suckCenterY = centerSuck4.Y;
                                        zSafePos = z4SafePos;
                                        break;
                                }
                                GTSCard.AxisPosMove(ref axisX, suckPicker.X - suckCenterX + deltaX, xRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, suckPicker.Y - suckCenterY + deltaY, yRunSpeed);
                                GTSCard.AxisPosMove(ref axisR, suckPicker.R, rRunSpeed);
                                stepnum = 1014;
                            }
                            break;
                        case 1014:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY) && GTSCard.AxisPosMoveCheckDone(axisR))
                                {
                                    stepnum = 1015;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisR, 0);
                                stepnum = 1013;
                            }
                            break;
                        case 1015:
                            if (!PauseFlag)
                            {
                                if (IsSimulate || plcModbus.MCoils_M2000[6 + suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex])
                                {
                                    GTSCard.SetDo(axisX.CardNo, (short)(4 + _curSuckPcs.PickIndex), 0);//开真空吸
                                    stepnum = 1016;
                                }
                            }
                            break;
                        case 1016:
                            if (!PauseFlag)
                            {
                                double pos1 = GTSCard.GetEnc(axisZ);
                                PTData[] data = new PTData[2];
                                if (suckPicker.Z > pos1)
                                {
                                    double dist = suckPicker.Z - pos1 - zSlowDistSuck;
                                    data[0] = default;
                                    data[0].pos = dist;
                                    data[0].time = (int)(dist / zRunSpeed * 1000);
                                    data[1] = default;
                                    data[1].pos = zSlowDistSuck;
                                    data[1].time = (int)(zSlowDistSuck / zSlowSpeedSuck * 1000);
                                }
                                else
                                {
                                    double dist = suckPicker.Z - pos1 - zSlowDistSuck * -1;
                                    data[0] = default;
                                    data[0].pos = dist;
                                    data[0].time = (int)(dist * -1 / zRunSpeed * 1000);
                                    data[1] = default;
                                    data[1].pos = zSlowDistSuck * -1;
                                    data[1].time = (int)(zSlowDistSuck / zSlowSpeedSuck * 1000);
                                }
                                GTSCard.AxisPTMove(ref axisZ, data, 0);
                                //GTSCard.AxisPosMove(ref axisZ, suckPicker.Z, zRunSpeed);
                                stepnum = 1017;
                            }
                            break;
                        case 1017:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    sw.Restart();
                                    stepnum = 1018;
                                }
                            }
                            break;
                        case 1018:
                            if (!PauseFlag)
                            {
                                if (sw.Elapsed.TotalSeconds > filmSuckTime)
                                {
                                    sw.Stop();
                                    stepnum = 1019;
                                }
                            }
                            break;
                        case 1019:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ, flyStartPicker.Z, zRunSpeed);//抬起来
                                stepnum = 1020;
                            }
                            break;
                        case 1020:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    if (GTSCard.GetDi(axisX.CardNo, (ushort)(0 + _curSuckPcs.PickIndex)) || IsSimulate)//判断真空吸
                                    {
                                        var counter = feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].Counter + 1;
                                        var columnCount = feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].ColumnCount;
                                        if (counter >= columnCount)
                                        {
                                            feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].Counter = 0;
                                            if (!IsSimulate)
                                            {
                                                plcModbus.SetM(1, (ushort)(2014 + suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex), true);
                                            }
                                        }
                                        else
                                        {
                                            feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].Counter = counter;
                                        }
                                        stepnum = 1002;
                                    }
                                    else
                                    {
                                        addMessage($"吸头{_curSuckPcs.PickIndex + 1}在供料{suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex + 1}真空报警!");
                                        aggregator.SendMessage($"下层:吸头{_curSuckPcs.PickIndex + 1}在供料{suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex + 1}真空报警!", "Alarm");
                                        stepnum = 1021;
                                    }

                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 1019;
                            }
                            break;
                        case 1021:
                            if (!PauseFlag)
                            {
                                if (resetButton)
                                {
                                    GTSCard.SetDo(axisX.CardNo, (short)(4 + _curSuckPcs.PickIndex), 1);
                                    var counter = feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].Counter + 1;
                                    var columnCount = feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].ColumnCount;
                                    if (counter >= columnCount)
                                    {
                                        feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].Counter = 0;
                                        if (!IsSimulate)
                                        {
                                            plcModbus.SetM(1, (ushort)(2014 + suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex), true);
                                            stepnum = 11021;
                                        }
                                        else
                                        {
                                            stepnum = 1013;
                                        }
                                    }
                                    else
                                    {
                                        feederWorkArr[suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex].Counter = counter;
                                        stepnum = 1013;
                                    }                                    
                                }
                            }
                            break;
                        case 11021:
                            if (!PauseFlag)
                            {
                                //等待剥膜信号复位
                                if (!plcModbus.MCoils_M2000[6 + suckerWorkArr[_curSuckPcs.PickIndex].FeederIndex])
                                {
                                    stepnum = 1013;
                                }
                            }
                            break;
                        #endregion
                        #region 下相机飞拍
                        case 1030:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ1, flyStartPicker.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ2, flyStartPicker.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ3, flyStartPicker.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ4, flyStartPicker.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisR1, flyStartPicker.R, rRunSpeed);
                                GTSCard.AxisPosMove(ref axisR2, flyStartPicker.R, rRunSpeed);
                                GTSCard.AxisPosMove(ref axisX, flyStartPicker.X, xRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, flyStartPicker.Y, yRunSpeed);
                                stepnum = 1031;
                            }
                            break;
                        case 1031:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ1) && GTSCard.AxisPosMoveCheckDone(axisZ2) && GTSCard.AxisPosMoveCheckDone(axisZ3) && GTSCard.AxisPosMoveCheckDone(axisZ4) &&
                                    GTSCard.AxisPosMoveCheckDone(axisR1) && GTSCard.AxisPosMoveCheckDone(axisR2) && GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY))
                                {
                                    camDown.ImageQueue.Clear();//开始拍照
                                    camDown.SetTriggerMode(1);
                                    camDown.StartGrabe();
                                    flySpeed = xRunSpeed > 1000 ? 1000 : xRunSpeed;
                                    double cameraGrabMovSpace = downCameraGrabPreTime / 1000 * flySpeed;//相机拍照时间认为是0.4ms，此处算的是拍照过程的偏移距离
                                    int[] Buf1 = new int[4];//飞拍压数据
                                    double dirfactor = flyStartPicker.X >= flyEndPicker.X ? -1 : 1;
                                    Buf1[0] = (int)((buttomCameraGrab_Sucker1.X - flyStartPicker.X - cameraGrabMovSpace * dirfactor) / axisX.Equiv);
                                    Buf1[1] = Buf1[0] + (int)(centerSuck2.X * dirfactor / axisX.Equiv);
                                    Buf1[2] = Buf1[0] + (int)(centerSuck3.X * dirfactor / axisX.Equiv);
                                    Buf1[3] = Buf1[0] + (int)(centerSuck4.X * dirfactor / axisX.Equiv);
                                    GTSCard.AxisCompareUse2(axisX.CardNo, 1, Buf1);

                                    filmCCDCalcFinish = false;
                                    if (!IsSimulate)
                                        Task.Run(() => {
                                            int imageIndex = 0;
                                            while (imageIndex < 4)
                                            {
                                                if (camDown.ImageQueue.TryPeek(out var img))
                                                {
                                                    //有照片                                                    
                                                    camDown.ImageQueue.TryDequeue(out img);
                                                    var item = pickerPcsList.FirstOrDefault(p => p.PickIndex == imageIndex);
                                                    if (item != null)
                                                    {
                                                        //有需要用的图片
                                                        try
                                                        {
                                                            item.FilmX = -999;
                                                            item.FilmY = -999;
                                                            item.FilmR = -999;
                                                            //item.PickIndex
                                                            HOperatorSet.ReduceDomain(img, recognizeRegions[item.PickIndex], out var imageReduced);
                                                            HOperatorSet.FindShapeModel(imageReduced, ModelIDs[item.PickIndex], new HTuple(-180).TupleRad(), new HTuple(360).TupleRad(), 0.5, 1, 0.5, "least_squares", new HTuple(5).TupleConcat(1), 0.75, out var Row, out var Column, out var Angle, out var Score);
                                                            if (Score.TupleLength() > 0)
                                                            {
                                                                //找到模板
                                                                HOperatorSet.HomMat2dIdentity(out var HomMat2D);
                                                                HOperatorSet.HomMat2dRotate(HomMat2D, Angle, 0, 0, out HomMat2D);
                                                                HOperatorSet.HomMat2dTranslate(HomMat2D, Row, Column, out HomMat2D);
                                                                HOperatorSet.AffineTransContourXld(ModelContourss[item.PickIndex], out var TransContours, HomMat2D);
                                                                HOperatorSet.VectorAngleToRigid(ShapeModelRows[item.PickIndex], ShapeModelColumns[item.PickIndex], ShapeModelAngles[item.PickIndex], Row, Column, Angle, out var ShapeHomMat2D);

                                                                HTuple _qx1, _qy1, _qx2, _qy2;
                                                                HOperatorSet.AffineTransPoint2d(ShapeHomMat2D, roiLine1s[item.PickIndex].row1, roiLine1s[item.PickIndex].col1, out _qx1, out _qy1);
                                                                HOperatorSet.AffineTransPoint2d(ShapeHomMat2D, roiLine1s[item.PickIndex].row2, roiLine1s[item.PickIndex].col2, out _qx2, out _qy2);

                                                                ImageCalc.MFindLine(img, out var roiLine1Region, _qx1, _qy1, _qx2, _qy2, 20, 30, 0.6, out var line1Row1, out var line1Column1, out var line1Row2, out var line1Column2, out var resultLine1);
                                                                if (resultLine1.I == 1)
                                                                {
                                                                    HOperatorSet.AffineTransPoint2d(ShapeHomMat2D, roiLine2s[item.PickIndex].row1, roiLine2s[item.PickIndex].col1, out _qx1, out _qy1);
                                                                    HOperatorSet.AffineTransPoint2d(ShapeHomMat2D, roiLine2s[item.PickIndex].row2, roiLine2s[item.PickIndex].col2, out _qx2, out _qy2);

                                                                    ImageCalc.MFindLine(img, out var roiLine2Region, _qx1, _qy1, _qx2, _qy2, 20, 30, 0.6, out var line2Row1, out var line2Column1, out var line2Row2, out var line2Column2, out var resultLine2);

                                                                    if (resultLine2.I == 1)
                                                                    {
                                                                        HOperatorSet.IntersectionLines(line1Row1, line1Column1, line1Row2, line1Column2, line2Row1, line2Column1, line2Row2, line2Column2, out var intersectionRow, out var intersectionColumn, out var isOverlapping);

                                                                        HOperatorSet.AngleLx(line1Row1, line1Column1, line1Row2, line1Column2, out var hv_Angle);

                                                                        HOperatorSet.AffineTransPoint2d(CalibHomMat2D, StandardRows[item.PickIndex], StandardColumns[item.PickIndex], out var standardx, out var standardy);

                                                                        HOperatorSet.AffineTransPoint2d(CalibHomMat2D, intersectionRow, intersectionColumn, out var newx, out var newy);

                                                                        HOperatorSet.VectorAngleToRigid(newx, newy, hv_Angle * -1, standardx, standardy, StandardAngles[item.PickIndex] * -1, out var homMat2D);

                                                                        double grabx = buttomCameraGrab_Sucker1.X, graby = buttomCameraGrab_Sucker1.Y;
                                                                        switch (item.PickIndex)
                                                                        {
                                                                            case 1:
                                                                                {
                                                                                    graby += centerSuck2.Y;
                                                                                }
                                                                                break;
                                                                            case 2:
                                                                                {
                                                                                    graby += centerSuck3.Y;
                                                                                }
                                                                                break;
                                                                            case 3:
                                                                                {
                                                                                    graby += centerSuck4.Y;
                                                                                }
                                                                                break;
                                                                            default:
                                                                                break;
                                                                        }

                                                                        HOperatorSet.AffineTransPoint2d(homMat2D, grabx, graby, out var calcX, out var calcY);

                                                                        double deltax = calcX.D - grabx;
                                                                        double deltay = calcY.D - graby;
                                                                        HTuple deltaangle = StandardAngles[item.PickIndex] * -1 - hv_Angle * -1;

                                                                        if (deltaangle > Math.PI)
                                                                        {
                                                                            deltaangle -= 2 * Math.PI;
                                                                        }
                                                                        if (deltaangle < Math.PI * -1)
                                                                        {
                                                                            deltaangle += 2 * Math.PI;
                                                                        }

                                                                        if (Math.Abs(deltax) <= xYGapLimit && Math.Abs(deltay) <= xYGapLimit && Math.Abs(deltaangle.TupleDeg().D) <= rGapLimit)
                                                                        {
                                                                            //没有超限
                                                                            item.FilmX = calcX.D;
                                                                            item.FilmY = calcY.D;
                                                                            item.FilmR = buttomCameraGrab_Sucker1.R + deltaangle.TupleDeg().D;
                                                                        }
                                                                        else
                                                                        {
                                                                            addMessage("膜图像识别距离超限!");
                                                                        }
                                                                        System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                                                                        {
                                                                            try
                                                                            {
                                                                                switch (item.PickIndex)
                                                                                {
                                                                                    case 0:
                                                                                        CameraIamge1 = new HImage(img);

                                                                                        CameraGCStyle1 = new Tuple<string, object>("Color", "yellow");
                                                                                        CameraGCStyle1 = new Tuple<string, object>("DrawMode", "margin");
                                                                                        CameraAppendHObject1 = recognizeRegions[item.PickIndex].CopyObj(1, -1);

                                                                                        CameraGCStyle1 = new Tuple<string, object>("Color", "green");
                                                                                        CameraGCStyle1 = new Tuple<string, object>("DrawMode", "margin");
                                                                                        CameraAppendHObject1 = TransContours;

                                                                                        CameraGCStyle1 = new Tuple<string, object>("Color", "cyan");
                                                                                        CameraGCStyle1 = new Tuple<string, object>("DrawMode", "fill");
                                                                                        CameraAppendHObject1 = roiLine1Region;

                                                                                        CameraAppendHObject1 = roiLine2Region;

                                                                                        CameraAppendHMessage1 = new HMsgEntry($"坐标:{intersectionRow.D:F1},{intersectionColumn.D:F1},{hv_Angle.TupleDeg().D:F1}°", 100, 100);
                                                                                        CameraAppendHMessage1 = new HMsgEntry($"计算结果 ΔX={deltax:F2}, ΔY={deltay:F1},ΔR={deltaangle.TupleDeg().D:F2}°", 300, 100);
                                                                                        break;
                                                                                    case 1:
                                                                                        CameraIamge2 = new HImage(img);

                                                                                        CameraGCStyle2 = new Tuple<string, object>("Color", "yellow");
                                                                                        CameraGCStyle2 = new Tuple<string, object>("DrawMode", "margin");
                                                                                        CameraAppendHObject2 = recognizeRegions[item.PickIndex].CopyObj(1, -1);

                                                                                        CameraGCStyle2 = new Tuple<string, object>("Color", "green");
                                                                                        CameraGCStyle2 = new Tuple<string, object>("DrawMode", "margin");
                                                                                        CameraAppendHObject2 = TransContours;

                                                                                        CameraGCStyle2 = new Tuple<string, object>("Color", "cyan");
                                                                                        CameraGCStyle2 = new Tuple<string, object>("DrawMode", "fill");
                                                                                        CameraAppendHObject2 = roiLine1Region;

                                                                                        CameraAppendHObject2 = roiLine2Region;

                                                                                        CameraAppendHMessage2 = new HMsgEntry($"坐标:{intersectionRow.D:F1},{intersectionColumn.D:F1},{hv_Angle.TupleDeg().D:F1}°", 100, 100);
                                                                                        CameraAppendHMessage2 = new HMsgEntry($"计算结果 ΔX={deltax:F2}, ΔY={deltay:F1},ΔR={deltaangle.TupleDeg().D:F2}°", 300, 100);
                                                                                        break;
                                                                                    case 2:
                                                                                        CameraIamge3 = new HImage(img);

                                                                                        CameraGCStyle3 = new Tuple<string, object>("Color", "yellow");
                                                                                        CameraGCStyle3 = new Tuple<string, object>("DrawMode", "margin");
                                                                                        CameraAppendHObject3 = recognizeRegions[item.PickIndex].CopyObj(1, -1);

                                                                                        CameraGCStyle3 = new Tuple<string, object>("Color", "green");
                                                                                        CameraGCStyle3 = new Tuple<string, object>("DrawMode", "margin");
                                                                                        CameraAppendHObject3 = TransContours;

                                                                                        CameraGCStyle3 = new Tuple<string, object>("Color", "cyan");
                                                                                        CameraGCStyle3 = new Tuple<string, object>("DrawMode", "fill");
                                                                                        CameraAppendHObject3 = roiLine1Region;

                                                                                        CameraAppendHObject3 = roiLine2Region;

                                                                                        CameraAppendHMessage3 = new HMsgEntry($"坐标:{intersectionRow.D:F1},{intersectionColumn.D:F1},{hv_Angle.TupleDeg().D:F1}°", 100, 100);
                                                                                        CameraAppendHMessage3 = new HMsgEntry($"计算结果 ΔX={deltax:F2}, ΔY={deltay:F1},ΔR={deltaangle.TupleDeg().D:F2}°", 300, 100);
                                                                                        break;
                                                                                    case 3:
                                                                                        CameraIamge4 = new HImage(img);

                                                                                        CameraGCStyle4 = new Tuple<string, object>("Color", "yellow");
                                                                                        CameraGCStyle4 = new Tuple<string, object>("DrawMode", "margin");
                                                                                        CameraAppendHObject4 = recognizeRegions[item.PickIndex].CopyObj(1, -1);

                                                                                        CameraGCStyle4 = new Tuple<string, object>("Color", "green");
                                                                                        CameraGCStyle4 = new Tuple<string, object>("DrawMode", "margin");
                                                                                        CameraAppendHObject4 = TransContours;

                                                                                        CameraGCStyle4 = new Tuple<string, object>("Color", "cyan");
                                                                                        CameraGCStyle4 = new Tuple<string, object>("DrawMode", "fill");
                                                                                        CameraAppendHObject4 = roiLine1Region;

                                                                                        CameraAppendHObject4 = roiLine2Region;

                                                                                        CameraAppendHMessage4 = new HMsgEntry($"坐标:{intersectionRow.D:F1},{intersectionColumn.D:F1},{hv_Angle.TupleDeg().D:F1}°", 100, 100);
                                                                                        CameraAppendHMessage4 = new HMsgEntry($"计算结果 ΔX={deltax:F2}, ΔY={deltay:F1},ΔR={deltaangle.TupleDeg().D:F2}°", 300, 100);
                                                                                        break;
                                                                                    default:
                                                                                        break;
                                                                                }
                                                                            }
                                                                            catch { }
                                                                        }));
                                                                    }
                                                                    else
                                                                    {
                                                                        addMessage($"吸嘴{item.PickIndex + 1}未找到直线2。");
                                                                        switch (imageIndex)
                                                                        {
                                                                            case 0:
                                                                                CameraIamge1 = new HImage(img);
                                                                                break;
                                                                            case 1:
                                                                                CameraIamge2 = new HImage(img);
                                                                                break;
                                                                            case 2:
                                                                                CameraIamge3 = new HImage(img);
                                                                                break;
                                                                            case 3:
                                                                                CameraIamge4 = new HImage(img);
                                                                                break;
                                                                            default:
                                                                                break;
                                                                        }
                                                                    }

                                                                }
                                                                else
                                                                {
                                                                    addMessage($"吸嘴{item.PickIndex + 1}未找到直线1。");
                                                                    switch (imageIndex)
                                                                    {
                                                                        case 0:
                                                                            CameraIamge1 = new HImage(img);
                                                                            break;
                                                                        case 1:
                                                                            CameraIamge2 = new HImage(img);
                                                                            break;
                                                                        case 2:
                                                                            CameraIamge3 = new HImage(img);
                                                                            break;
                                                                        case 3:
                                                                            CameraIamge4 = new HImage(img);
                                                                            break;
                                                                        default:
                                                                            break;
                                                                    }
                                                                }

                                                            }
                                                            else
                                                            {
                                                                addMessage($"吸嘴{item.PickIndex + 1}未找到模板。");
                                                                switch (imageIndex)
                                                                {
                                                                    case 0:
                                                                        CameraIamge1 = new HImage(img);
                                                                        break;
                                                                    case 1:
                                                                        CameraIamge2 = new HImage(img);
                                                                        break;
                                                                    case 2:
                                                                        CameraIamge3 = new HImage(img);
                                                                        break;
                                                                    case 3:
                                                                        CameraIamge4 = new HImage(img);
                                                                        break;
                                                                    default:
                                                                        break;
                                                                }
                                                            }
                                                        }
                                                        catch (Exception ex)
                                                        {
                                                            addMessage(ex.Message);
                                                            switch (imageIndex)
                                                            {
                                                                case 0:
                                                                    CameraIamge1 = new HImage(img);
                                                                    break;
                                                                case 1:
                                                                    CameraIamge2 = new HImage(img);
                                                                    break;
                                                                case 2:
                                                                    CameraIamge3 = new HImage(img);
                                                                    break;
                                                                case 3:
                                                                    CameraIamge4 = new HImage(img);
                                                                    break;
                                                                default:
                                                                    break;
                                                            }
                                                        }
                                                    }
                                                    else
                                                    {
                                                        switch (imageIndex)
                                                        {
                                                            case 0:
                                                                CameraIamge1 = new HImage(img);
                                                                break;
                                                            case 1:
                                                                CameraIamge2 = new HImage(img);
                                                                break;
                                                            case 2:
                                                                CameraIamge3 = new HImage(img);
                                                                break;
                                                            case 3:
                                                                CameraIamge4 = new HImage(img);
                                                                break;
                                                            default:
                                                                break;
                                                        }
                                                    }
                                                    imageIndex++;
                                                }
                                                Thread.Sleep(100);
                                            }
                                        }).ContinueWith(arg => { filmCCDCalcFinish = true; });
                                    else
                                        Task.Run(() =>
                                        {
                                            int imageIndex = 0;
                                            while (imageIndex < 4)
                                            {
                                                if (camDown.ImageQueue.TryPeek(out var img))
                                                {
                                                    //有照片                                                    
                                                    camDown.ImageQueue.TryDequeue(out img);
                                                    switch (imageIndex)
                                                    {
                                                        case 0:
                                                            CameraIamge1 = new HImage(img);
                                                            break;
                                                        case 1:
                                                            CameraIamge2 = new HImage(img);
                                                            break;
                                                        case 2:
                                                            CameraIamge3 = new HImage(img);
                                                            break;
                                                        case 3:
                                                            CameraIamge4 = new HImage(img);
                                                            break;
                                                        default:
                                                            break;
                                                    }
                                                    imageIndex++;
                                                }
                                                Thread.Sleep(100);
                                            }
                                        });
                                    stepnum = 1032;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ1, 0);
                                GTSCard.AxisStop(axisZ2, 0);
                                GTSCard.AxisStop(axisZ3, 0);
                                GTSCard.AxisStop(axisZ4, 0);
                                GTSCard.AxisStop(axisR1, 0);
                                GTSCard.AxisStop(axisR2, 0);
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                stepnum = 1030;
                            }
                            break;
                        case 1032:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, flyEndPicker.X, xRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, flyEndPicker.Y, yRunSpeed);
                                stepnum = 1033;
                            }
                            break;
                        case 1033:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY))
                                {                                   
                                    stepnum = 11050;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                stepnum = 1032;
                            }
                            break;
                        case 11050:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ1, z1SafePos, zRunSpeed);//抬起来
                                GTSCard.AxisPosMove(ref axisZ2, z2SafePos, zRunSpeed);//抬起来
                                GTSCard.AxisPosMove(ref axisZ3, z3SafePos, zRunSpeed);//抬起来
                                GTSCard.AxisPosMove(ref axisZ4, z4SafePos, zRunSpeed);//抬起来
                                stepnum = 11051;
                            }
                            break;
                        case 11051:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ1) && GTSCard.AxisPosMoveCheckDone(axisZ2) && GTSCard.AxisPosMoveCheckDone(axisZ3) && GTSCard.AxisPosMoveCheckDone(axisZ4))
                                {
                                    stepnum = 11052;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ1, 0);
                                GTSCard.AxisStop(axisZ2, 0);
                                GTSCard.AxisStop(axisZ3, 0);
                                GTSCard.AxisStop(axisZ4, 0);
                                stepnum = 11050;
                            }
                            break;
                        case 11052:
                            if (!PauseFlag)
                            {
                                if (IsSimulate || (!IsSimulate && filmCCDCalcFinish))
                                {
                                    var fail = pickerPcsList.FirstOrDefault(p => p.FilmX == -999);
                                    if (fail == null || IsSimulate)
                                    {
                                        if (isProductFlyGrabed)//已经飞拍过了
                                        {
                                            stepnum = 1050;//去贴料
                                        }
                                        else
                                        {
                                            CameraIndex = 0;
                                            stepnum = 10;//去飞拍
                                        }
                                    }
                                    else
                                    {
                                        //有拍照失败的，去抛料                                            
                                        stepnum = 11060;
                                    }
                                }
                            }
                            break;
                        #endregion
                        #region 抛料
                        case 11060:
                            releaseFailPcsQueue.Clear();
                            foreach (var pcs in pickerPcsList)
                            {
                                if (pcs.FilmX == -999)
                                {
                                    suckPcsQueue.Enqueue(pcs);
                                    releaseFailPcsQueue.Enqueue(pcs);
                                }
                            }
                            stepnum = 11061;
                            break;
                        case 11061:
                            if (releaseFailPcsQueue.TryPeek(out var releaseItem))
                            {
                                //_curReleasePcs
                                _curReleasePcs = releaseFailPcsQueue.Dequeue();
                                stepnum = 11062;
                            }
                            else
                            {
                                stepnum = 1002;//重新取料
                            }
                            break;
                        case 11062:
                            if (!PauseFlag)
                            {
                                double releasex = bottomCameraFail_Sucker1.X, releasey = bottomCameraFail_Sucker1.Y;
                                axisZ = axisZ1;
                                axisR = axisR1;
                                zSafePos = z1SafePos;
                                switch (_curReleasePcs.PickIndex)
                                {
                                    case 1:
                                        {
                                            releasex -= centerSuck2.X;
                                            releasey -= centerSuck2.Y;
                                            zSafePos = z2SafePos;
                                            axisZ = axisZ2;
                                            axisR = axisR1;
                                        }
                                        break;
                                    case 2:
                                        {
                                            releasex -= centerSuck3.X;
                                            releasey -= centerSuck3.Y;
                                            zSafePos = z3SafePos;
                                            axisZ = axisZ3;
                                            axisR = axisR2;
                                        }
                                        break;
                                    case 3:
                                        {
                                            releasex -= centerSuck4.X;
                                            releasey -= centerSuck4.Y;
                                            zSafePos = z4SafePos;
                                            axisZ = axisZ4;
                                            axisR = axisR2;
                                        }
                                        break;
                                    default:
                                        break;
                                }
                                Random rd1 = new Random();
                                double deltax1 = (double)(rd1.Next(100) - 50) / 50 * 50;
                                double deltay1 = (double)(rd1.Next(100) - 50) / 50 * 5;
                                GTSCard.AxisPosMove(ref axisX, releasex + deltax1, xRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, releasey + deltay1, yRunSpeed);
                                GTSCard.AxisPosMove(ref axisR, bottomCameraFail_Sucker1.R, rRunSpeed);
                                stepnum = 11063;
                            }
                            break;
                        case 11063:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY) && GTSCard.AxisPosMoveCheckDone(axisR))
                                {
                                    stepnum = 11064;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisR, 0);
                                stepnum = 11062;
                            }
                            break;
                        case 11064:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ, bottomCameraFail_Sucker1.Z, zRunSpeed);
                                stepnum = 11065;
                            }
                            break;
                        case 11065:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    GTSCard.SetDo(axisX.CardNo, (short)(4 + _curReleasePcs.PickIndex), 1);//关真空吸
                                    GTSCard.SetDo(axisX.CardNo, (short)(8 + _curReleasePcs.PickIndex), 0);//开破空吸
                                    sw.Restart();
                                    stepnum = 11066;
                                }
                            }
                            break;
                        case 11066:
                            if (!PauseFlag)
                            {
                                if (sw.Elapsed.TotalSeconds > 0.5)
                                {
                                    sw.Stop();
                                    GTSCard.SetDo(axisX.CardNo, (short)(8 + _curReleasePcs.PickIndex), 1);//关破空吸
                                    stepnum = 11067;
                                }
                            }
                            break;
                        case 11067:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ, zSafePos, zRunSpeed);//抬起来
                                stepnum = 11068;
                            }
                            break;
                        case 11068:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    stepnum = 11061;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 11067;
                            }
                            break;
                        #endregion
                        #region 贴料
                        case 1050:
                            {
                                foreach (var pcs in pickerPcsList)
                                {
                                    pastePcsQueue.Enqueue(pcs);
                                }
                                stepnum = 1051;
                            }
                            break;
                        case 1051:
                            if (pastePcsQueue.TryPeek(out var pastePcs))
                            {
                                _curPastePcs = pastePcsQueue.Dequeue();

                                //通知上层去辅助位置
                                aggregator.GetEvent<AssistPointMessageEvent>().Publish(new AssistPointMessageModel()
                                {
                                    X = _curPastePcs.X1,
                                    Y = _curPastePcs.Y1,
                                    Z = _curPastePcs.Z1,
                                    R = _curPastePcs.R1,
                                    ZIndex = _curPastePcs.PressIndex
                                });
                                stepnum = 10100;//计算移动相机
                            }
                            else
                            {
                                aggregator.SendMessage("BottomLoopFinished", "Run");//通知上层这轮结束了
                                stepnum = 1001;
                            }
                            break;
                        case 10100:
                            if (!IsSimulate)
                            {
                                double pcsr = _curPastePcs.R + UpCameraDeltaAngle.TupleDeg();
                                HOperatorSet.AffineTransPoint2d(UpCameraHomMat2D, _curPastePcs.X, _curPastePcs.Y, out var pcsx, out var pcsy);
                                HTuple homMat2D_2;//拍照点到贴合点的变换T2
                                double grabx = buttomCameraGrab_Sucker1.X;
                                double graby = buttomCameraGrab_Sucker1.Y;
                                double grabr = buttomCameraGrab_Sucker1.R;

                                switch (_curPastePcs.PickIndex)
                                {
                                    case 1:
                                        graby += centerSuck2.Y;
                                        break;
                                    case 2:
                                        graby += centerSuck3.Y;
                                        break;
                                    case 3:
                                        graby += centerSuck4.Y;
                                        break;
                                }
                                HOperatorSet.VectorAngleToRigid(grabx, graby, new HTuple(grabr).TupleRad(), pcsx, pcsy, new HTuple(pcsr).TupleRad(), out homMat2D_2);
                                HTuple finalx, finaly;
                                HOperatorSet.AffineTransPoint2d(homMat2D_2, _curPastePcs.FilmX, _curPastePcs.FilmY, out finalx, out finaly);
                                _curPastePcs.PcsX = finalx.D;
                                _curPastePcs.PcsY = finaly.D;
                                _curPastePcs.PcsR = pcsr + _curPastePcs.FilmR - grabr;
                                addMessage($"计算结果: X:{_curPastePcs.PcsX:F3}; Y:{_curPastePcs.PcsY:F3}; R:{_curPastePcs.PcsR:F2}; Z:{_curPastePcs.Z:F3}");
                            }
                            stepnum = 1052;
                            break;
                        case 1052:
                            if (!PauseFlag)
                            {
                                switch (_curPastePcs.PickIndex)
                                {
                                    case 0:
                                        axisZ = axisZ1;
                                        axisR = axisR1;
                                        suckCenterX = 0;
                                        suckCenterY = 0;
                                        zSafePos = z1SafePos;
                                        break;
                                    case 1:
                                        axisZ = axisZ2;
                                        axisR = axisR1;
                                        suckCenterX = centerSuck2.X;
                                        suckCenterY = centerSuck2.Y;
                                        zSafePos = z2SafePos;
                                        break;
                                    case 2:
                                        axisZ = axisZ3;
                                        axisR = axisR2;
                                        suckCenterX = centerSuck3.X;
                                        suckCenterY = centerSuck3.Y;
                                        zSafePos = z3SafePos;
                                        break;
                                    case 3:
                                    default:
                                        axisZ = axisZ4;
                                        axisR = axisR2;
                                        suckCenterX = centerSuck4.X;
                                        suckCenterY = centerSuck4.Y;
                                        zSafePos = z4SafePos;
                                        break;
                                }
                                if (!IsSimulate)
                                {
                                    GTSCard.AxisPosMove(ref axisX, _curPastePcs.PcsX - suckCenterX, xRunSpeed);
                                    GTSCard.AxisPosMove(ref axisY, _curPastePcs.PcsY - suckCenterY, yRunSpeed);
                                    GTSCard.AxisPosMove(ref axisR, _curPastePcs.PcsR, rRunSpeed);
                                }
                                else
                                {
                                    GTSCard.AxisPosMove(ref axisX, _curPastePcs.X - suckCenterX, xRunSpeed);
                                    GTSCard.AxisPosMove(ref axisY, _curPastePcs.Y - suckCenterY, yRunSpeed);
                                    GTSCard.AxisPosMove(ref axisR, _curPastePcs.R, rRunSpeed);
                                }
                                stepnum = 1053;
                            }
                            break;
                        case 1053:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY) && GTSCard.AxisPosMoveCheckDone(axisR))
                                {
                                    stepnum = 1054;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisR, 0);
                                stepnum = 1052;
                            }
                            break;

                        case 1054:
                            if (!PauseFlag)
                            {
                                if (AssistFinished)//上层已经到辅助位置
                                {
                                    double pos1 = GTSCard.GetEnc(axisZ);
                                    PTData[] data = new PTData[2];
                                    if (_curPastePcs.Z > pos1)
                                    {
                                        double dist = _curPastePcs.Z - pos1 - zSlowDistPaste;
                                        data[0] = default;
                                        data[0].pos = dist;
                                        data[0].time = (int)(dist / zRunSpeed * 1000);
                                        data[1] = default;
                                        data[1].pos = zSlowDistPaste;
                                        data[1].time = (int)(zSlowDistPaste / zSlowSpeedPaste * 1000);
                                    }
                                    else
                                    {
                                        double dist = _curPastePcs.Z - pos1 - zSlowDistPaste * -1;
                                        data[0] = default;
                                        data[0].pos = dist;
                                        data[0].time = (int)(dist * -1 / zRunSpeed * 1000);
                                        data[1] = default;
                                        data[1].pos = zSlowDistPaste * -1;
                                        data[1].time = (int)(zSlowDistPaste / zSlowSpeedPaste * 1000);
                                    }
                                    GTSCard.AxisPTMove(ref axisZ, data, 0);
                                    //GTSCard.AxisPosMove(ref axisZ, _curPastePcs.Z, zRunSpeed);
                                    stepnum = 1055;
                                }
                            }
                            break;
                        case 1055:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    AssistFinished = false;

                                    GTSCard.SetDo(axisX.CardNo, (short)(4 + _curPastePcs.PickIndex), 1);//关真空吸
                                    GTSCard.SetDo(axisX.CardNo, (short)(8 + _curPastePcs.PickIndex), 0);//开破空吸
                                    sw.Restart();
                                    stepnum = 1056;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 1054;
                            }
                            break;
                        case 1056:
                            if (!PauseFlag)
                            {
                                if (sw.Elapsed.TotalSeconds > filmReleaseTime)
                                {
                                    sw.Stop();
                                    GTSCard.SetDo(axisX.CardNo, (short)(8 + _curPastePcs.PickIndex), 1);//关破空吸
                                    aggregator.SendMessage("AssistUp", "Run");//通知上层可以抬起来了
                                    stepnum = 1057;
                                }
                            }
                            break;
                        case 1057:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisZ, zSafePos, zRunSpeed);//抬起来                               
                                stepnum = 1058;
                            }
                            break;
                        case 1058:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisZ))
                                {
                                    stepnum = 1051;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisZ, 0);
                                stepnum = 1057;
                            }
                            break;
                        #endregion
                        #region 下层贴完
                        case 1200:
                            if (!PauseFlag)
                            {
                                //通知上层，下层已经贴完
                                aggregator.SendMessage("BottomWordFinished", "Run");
                                stepnum = 1201;
                            }
                            break;
                        case 1201:
                            if (!PauseFlag)
                            {
                                if (TopWordFinished)
                                {
                                    TopWordFinished = false;
                                    RailReadyFlag = false;
                                    isProductFlyGrabed = false;//要重新拍产品
                                    stepnum = 1300;
                                }
                            }
                            break;
                        #endregion

                        #region 回初始位
                        case 1300:
                            if (!PauseFlag)
                            {
                                GTSCard.AxisPosMove(ref axisX, initPoint.X, xRunSpeed);
                                GTSCard.AxisPosMove(ref axisY, initPoint.Y, yRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ1, initPoint.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ2, initPoint.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ3, initPoint.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisZ4, initPoint.Z, zRunSpeed);
                                GTSCard.AxisPosMove(ref axisR1, initPoint.R, rRunSpeed);
                                GTSCard.AxisPosMove(ref axisR2, initPoint.R, rRunSpeed);
                                stepnum = 1301;
                            }
                            break;
                        case 1301:
                            if (!PauseFlag)
                            {
                                if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY)
                                    && GTSCard.AxisPosMoveCheckDone(axisZ1) && GTSCard.AxisPosMoveCheckDone(axisZ2) && GTSCard.AxisPosMoveCheckDone(axisZ3) && GTSCard.AxisPosMoveCheckDone(axisZ4)
                                    && GTSCard.AxisPosMoveCheckDone(axisR1) && GTSCard.AxisPosMoveCheckDone(axisR2))
                                {

                                    stepnum = 9;
                                }
                            }
                            else
                            {
                                GTSCard.AxisStop(axisX, 0);
                                GTSCard.AxisStop(axisY, 0);
                                GTSCard.AxisStop(axisZ1, 0);
                                GTSCard.AxisStop(axisZ2, 0);
                                GTSCard.AxisStop(axisZ3, 0);
                                GTSCard.AxisStop(axisZ4, 0);
                                GTSCard.AxisStop(axisR1, 0);
                                GTSCard.AxisStop(axisR2, 0);
                                stepnum = 1300;
                            }
                            break;
                        #endregion
                        default:
                            break;
                    }
                    #endregion
                    #region 更新界面
                    if (cycleCount++ > 100)
                    {
                        cycleCount = 0;
                        StepNum = stepnum;
                    }
                    #endregion
                }
                catch (Exception ex)
                {

                }
                System.Threading.Thread.Sleep(10);
            }
        }
        private void ResetAction()
        {
            GTSCard.AxisStop(Bottom_A8.A1, 0);
            GTSCard.AxisStop(Bottom_A8.A2, 0);
            GTSCard.AxisStop(Bottom_A8.A4, 0);
            GTSCard.AxisStop(Bottom_A8.A5, 0);
            GTSCard.AxisStop(Bottom_A8.A6, 0);
            GTSCard.AxisStop(Bottom_A8.A7, 0);
            GTSCard.AxisStop(Top_A4.A3, 0);
            GTSCard.AxisStop(Top_A4.A4, 0);
            GTSCard.ServoOff(Bottom_A8.A1);
            GTSCard.ServoOff(Bottom_A8.A2);
            GTSCard.ServoOff(Bottom_A8.A4);
            GTSCard.ServoOff(Bottom_A8.A5);
            GTSCard.ServoOff(Bottom_A8.A6);
            GTSCard.ServoOff(Bottom_A8.A7);
            GTSCard.ServoOff(Top_A4.A3);
            GTSCard.ServoOff(Top_A4.A4);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 0, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 1, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 2, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 3, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 4, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 5, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 6, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 7, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 8, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 9, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 10, 1);
            GTSCard.SetDo(Bottom_A8.A1.CardNo, 11, 1);
        }
        private void XHome(CancellationToken token)
        {
            int stepnum = 0;
            var axisX = Bottom_A8.A1;
            addMessage("X轴回原点中");
            while (true)
            {
                if (token.IsCancellationRequested)
                {
                    return;
                }
                switch (stepnum)
                {
                    case 0:
                        if (!XHomed)
                        {
                            stepnum = 3;
                        }
                        else
                        {
                            addMessage("X轴已回原点");
                            return;
                        }
                        break;
                    case 3:
                        if (!PauseFlag)
                        {
                            GTSCard.AxisHomeMove(axisX);
                            stepnum = 4;
                        }
                        break;
                    case 4:
                        if (!PauseFlag)
                        {
                            if (GTSCard.AxisHomeCheckDone(axisX))
                            {
                                System.Threading.Thread.Sleep(1000);
                                GTSCard.AxisZeroSet(Bottom_A8.A1);
                                GTSCard.AxisPosSet(Bottom_A8.A1, 1215);
                                GTSCard.AxisEncSet(Bottom_A8.A1, 1215);
                                stepnum = 5;
                            }
                        }
                        else
                        {
                            GTSCard.AxisStop(Bottom_A8.A1, 0);
                            stepnum = 3;
                        }
                        break;
                    case 5:
                        if (!PauseFlag)
                        {

                            GTSCard.AxisPosMove(ref axisX, 1205, axisX.HomeHSpd);
                            stepnum = 6;
                        }
                        break;
                    case 6:
                        if (!PauseFlag)
                        {
                            if (GTSCard.AxisPosMoveCheckDone(axisX))
                            {
                                System.Threading.Thread.Sleep(200);
                                GTSCard.ClearAlm1(axisX);
                                XHomed = true;
                                addMessage("X轴回原点完成");
                                return;
                            }
                        }
                        else
                        {
                            GTSCard.AxisStop(axisX, 1);
                            stepnum = 5;
                        }
                        break;
                }
                System.Threading.Thread.Sleep(100);
            }
        }
        private void YHome(CancellationToken token)
        {
            int stepnum = 0;
            addMessage("Y轴回原点中");
            while (true)
            {
                if (token.IsCancellationRequested)
                {
                    return;
                }
                switch (stepnum)
                {
                    case 0:
                        if (!YHomed)
                        {
                            stepnum = 3;
                        }
                        else
                        {
                            addMessage("Y轴已回原点");
                            return;
                        }
                        break;
                    case 3:
                        if (!PauseFlag)
                        {
                            GTSCard.AxisHomeMove(Bottom_A8.A2);
                            stepnum = 4;
                        }
                        break;
                    case 4:
                        if (!PauseFlag)
                        {
                            if (GTSCard.AxisHomeCheckDone(Bottom_A8.A2))
                            {
                                System.Threading.Thread.Sleep(1000);
                                GTSCard.AxisZeroSet(Bottom_A8.A2);
                                YHomed = true;
                                addMessage("Y轴回原点完成");
                                return;
                            }
                        }
                        else
                        {
                            GTSCard.AxisStop(Bottom_A8.A2, 0);
                            stepnum = 3;
                        }
                        break;
                }
                System.Threading.Thread.Sleep(100);
            }
        }
        #endregion
    }

}
